Modified Kalman–Bucy filter for attitude and pose quasi-optimal estimation

This work presents two versions of a novel modified formulation of the Kalman–Bucy filter where sensor measurements appear in the covariance matrix dynamics. The first formulation presented in this paper is an attitude quaternion-based formulation which quasi-optimally estimates the relative attitude between two frames. The second formulation builds on the attitude quaternion-based filter by extending the results to dual quaternions, which allows the filter to quasi-optimally estimate the pose between two frames. The utility of this filter is attributed to the fact the filter will quasi-optimally estimate the attitude or pose between two rigid bodies provided the measurements of any two quantities as long as the quantities are measured in both frames. Additionally, both formulations remain in quaternion and dual quaternion space which retains the computational efficiency of quaternion and dual quaternion algebra. Lastly, simulations were run in MATLAB which demonstrate the utility of both formulations.