Second-Order-Optimal Filter on Lie Groups for Planar Rigid Bodies

Attitude estimation is a core problem in many mobile robotic systems, such as unmanned aerial and ground vehicles. The configuration space of these systems is properly modeled by exploiting the theory of Lie groups. In this article, we propose a second-order-optimal minimum-energy filter on the matrix Lie group TSE(2), the tangent bundle of the special Euclidean group SE(2), where the optimality is with respect to a cost function in the unknown input and output error measurements. In this article, the measurement equation consists of a global positioning system-like device with two antennas attached to the planar rigid body and an inertial measurement unit-like device. Even though the mathematics is quite complicated, the accuracy of the filter justifies this approach. Simulations show the effectiveness of the proposed filter.