Second-order correction and numerical considerations for the two-step optimal estimator

A modification of the two-step optimal filter is presented. The two-step filter is an alternative to the standard recursive estimators that are applied to nonlinear measurement problems, such as the extended and iterated extended Kalman filters. It improves the estimate error by splitting the cost function minimization into two steps (a linear first step and a nonlinear second step) by defining a set of first-step states that are nonlinear combinations of the desired states. A linear approximation is made in the time update of the first-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 terms in the state estimate error is presented. Previous work used a first-order expansion of the nonlinear function relating first- and second-step states to find the time update of the first-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 filter algorithm is also presented that improves the robustness and accuracy of the filter. Performance is verified using a radar ranging example.