Covariance Correction Step for Kalman Filtering with an Attitude

Redundant attitude representations are often used in Kalman filters for estimating dynamic states that include an attitude. A minimal three-element attitude deviation is combined with a reference attitude, where the deviation is included in the filter state and has an associated covariance estimate. This paper derives a reset step that adjusts the covariance matrix when information is moved from the attitude deviation to the reference attitude. When combined with the extended or unscented Kalman filter prediction and measurement steps, the reset allows one to easily construct a Kalman filter for a system for which the state includes an attitude. This algorithm is closely related to (and a correction to) the multiplicative extended Kalman filter or the unscented quaternion estimator, depending on whether the reset is combined with an extended or unscented Kalman filter. In comparison to the multiplicative extended Kalman filter, it is more general and includes a reset after the measurement update, as well as a reset after both the prediction and measurement update steps of the unscented quaternion estimator. This reset step is derived by tracking the mean and covariance through a linearization, similar to an extended Kalman filter prediction step. The reset step is validated using Monte Carlo sampling.