Complementary filter design on the Special Euclidean group SE(3)

This paper considers the problem of obtaining high quality pose estimation (position and orientation) from a combination of low cost sensors, such as an inertial measurement unit and vision sensor. A non-linear complementary filter is proposed that evolves on the Special Euclidean Group SE(3). Exponential stability of the filter is proved. Simulation results are presented to illustrate simplicity and demonstrate the performance of the proposed approach. Experimental results reinforce the convergence of the filter.