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 modelled exploiting the theory of Lie groups. In this paper 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 work, the measurement equation consists of a GPS-like (Global Positioning System) device with two antennas attached to the planar rigid body and an IMU-like (Inertial Measurement Unit). Even though the mathematics is quite complicated, the accuracy of the filter justifies this approach. Simulations show the effectiveness of the proposed filter.