Enforcing an Algebraic Constraint in Extended Kalman Filter Design

Abstract

A new method is presented for optimally enforcing an algebraic constraint in Kalman filtering. The method is illustrated in the setting of inertial navigation using extended Kalman filtering. It is shown that to first order in the estimation error the constrained optimal update of the quaternion portion of the state vector is a projection of the unconstrained update onto the constraint space, and the update increment for the quaternion is perpendicular to the a priori estimate. The procedure is easy to implement and appears to provide a nearly optimal solution to the problem of quaternion normalization in the setting of inertial navigation. More importantly, it provides a correction for the covariance update that significantly enhances filter stability. Examples involving initial alignment and inertial navigation illustrate the efficacy of the approach, and the method is compared to several other methods in the literature.