EKF-Based Recursive Dual Estimation of Structure and Motion From Stereo Data

The estimation of motion and structure from stereo-video streams is revisited for applications in ocean exploration and seafloor mapping. Operational constraints require real-time processing to enable adaptive trajectory planning, and robust estimation to navigate optimal paths for collection of useful underwater stereo data. While the traditional joint estimation of motion and structure by way of an extended Kalman filter (EKF) provides a suitable recursive framework, the cubic computation growth with the number of feature tracks is a serious bottleneck. By treating the motion and structure as the states of two coupled filters with stereo feature correspondences as observations, a dual estimator is devised with the performance of joint estimation and computational complexity proportional to the number of features. We favor a sequential implementation to ensure unbiased estimation, in contrast to two parallel dual estimation schemes that generally produce biased updates. Stochastic stability can be established in terms of conditions on initial estimation error, bound on observation noise covariance, observation nonlinearity, and modeling error. Moreover, dynamic features can be treated effectively and efficiently by the removal or addition to a bank of filters, one assigned per feature. Experimental results with synthetic and several real data sets are presented to demonstrate the merits of the proposed recursive dual EKF-based estimator.