Second-Order Correction and Numerical Considerations for the Two-Step Optimal Estimator

A modie cation of the two-step optimal e lter is presented. The two-step e lter is an alternative to the standard recursive estimators that are applied to nonlinear measurement problems, such as the extended and iterated extended Kalman e lters. It improves the estimate error by splitting the cost function minimization into two steps (a linear e rst step and a nonlinearsecond step )by dee ning a set of e rst-step states that are nonlinear combinations of the desired states. A linear approximation is made in the time update of the e rst-step states rather than in the measurement update as in conventional methods. An extension of that approximation of the time update by including higher-order termsin thestateestimateerrorispresented. Previous work used a e rst-orderexpansion of the nonlinear function relating e rst- and second-step states to e nd the time update of the e rst-step states. Terms to third order in the estimate error are retained here resulting in a time update keeping second-order corrections in both the state estimate error and the covariance. The result is an estimate with lower bias and lower mean square error. A square root implementation of the two-step e lter algorithm is also presented that improves the robustness and accuracy of the e lter. Performance is verie ed using a radar ranging example.