The Iterated Sigma Point Kalman Filter with Applications to Long Range Stereo

This paper investigates the use of statistical linearization to improve iterative non-linear least squares estimators. In particular, we look at improving long range stereo by filtering feature tracks from sequences of stereo pairs. A novel filter called the Iterated Sigma Point Kalman Filter (ISPKF) is developed from first principles; this filter is shown to achieve superior performance in terms of efficiency and accuracy when compared to the Extended Kalman Filter (EKF), Unscented Kalman Filter (UKF), and Gauss-Newton filter. We also compare the ISPKF to the optimal Batch filter and to a Gauss-Newton Smoothing filter. For the long range stereo problem the ISPKF comes closest to matching the performance of the full batch MLE estimator. Further, the ISPKF is demonstrated on real data in the context of modeling environment structure from long range stereo data.