Asymptotically Stable Optimal Multi-rate Rigid Body Attitude Estimation based on Lagrange-d'Alembert Principle
The rigid body attitude estimation problem is treated using the discrete-time Lagrange-d'Alembert principle. Three different possibilities are considered for the multi-rate relation between angular velocity measurements and direction vector measurements for attitude: 1) integer relation between sampling rates, 2) time-varying sampling rates, 3) non-integer relation between sampling rates. In all cases, it is assumed that angular velocity measurements are sampled at a higher rate compared to the inertial vectors. The attitude determination problem from two or more vector measurements in the body-fixed frame is formulated as Wahba's problem. At instants when direction vector measurements are absent, a discrete-time model for attitude kinematics is used to propagate past measurements. A discrete-time Lagrangian is constructed as the difference between a kinetic energy-like term that is quadratic in the angular velocity estimation error and an artificial potential energy-like term obtained from Wahba's cost function. An additional dissipation term is introduced and the discrete-time Lagrange-d'Alembert principle is applied to the Lagrangian with this dissipation to obtain an optimal filtering scheme. A discrete-time Lyapunov analysis is carried out to show that the optimal filtering scheme is asymptotically stable in the absence of measurement noise and the domain of convergence is almost global. For a realistic evaluation of the scheme, numerical experiments are conducted with inputs corrupted by bounded measurement noise. These numerical simulations exhibit convergence of the estimated states to a bounded neighborhood of the actual states.