A second order minimum-energy filter on the special orthogonal group

This work documents a case study in the application of Mortensen´s nonlinear filtering approach to invariant systems on general Lie groups. In this paper, we consider the special orthogonal group SO(3) of all rotation matrices. We identify the exact form of the kinematics of the minimum-energy (optimal) observer on SO(3) and note that it depends on the Hessian of the value function of the associated optimal control problem. We derive a second order approximation of the dynamics of the Hessian by neglecting third order terms in the expansion of the dynamics. This yields a Riccati equation that together with the optimal observer equation form a second order minimum-energy filter on SO(3). The proposed filter is compared to the multiplicative extended Kalman filter (MEKF), arguably the industry standard for attitude estimation, by means of simulations. Our studies indicate superior transient and asymptotic tracking performance of the proposed filter as compared to the MEKF.