Discrete-time Rigid Body Attitude State Estimation based on the Discrete Lagrange-d' Alembert Principle

Discrete-time estimation of rigid body attitude and angular velocity without any knowledge of the attitude dynamics model, is treated using the discrete Lagrange-d'Alembert principle. Using body-fixed sensor measurements of direction vectors and angular velocity, a Lagrangian is obtained as the difference between a kinetic energy-like term that is quadratic in the angular velocity estimation error and an artificial potential obtained from Wahba's function. An additional dissipation term that depends on the angular velocity estimation error is introduced, and the discrete Lagrange-d'Alembert principle is applied to the Lagrangian with this dissipation. An explicit first order and a symmetric second-order version of this discrete-time filtering scheme are also presented, with a discussion of their advantages. A numerical simulation comparing the performances of the second-order estimator and the first-order estimator is carried out.