Rigid body pose estimation based on the Lagrange–d’Alembert principle

Abstract

Stable estimation of rigid body pose and velocities from noisy measurements, without any knowledge of the dynamics model, is treated using the Lagrange-d'Alembert principle from variational mechanics. With body-fixed optical and inertial sensor measurements, a Lagrangian is obtained as the difference between a kinetic energy-like term that is quadratic in velocity estimation error and the sum of two artificial potential functions; one obtained from a generalization of Wahba's function for attitude estimation and another which is quadratic in the position estimate error. An additional dissipation term that is linear in the velocity estimation error is introduced, and the Lagrange-d'Alembert principle is applied to the Lagrangian with this dissipation. A Lyapunov analysis shows that the state estimation scheme so obtained provides stable asymptotic convergence of state estimates to actual states in the absence of measurement noise, with an almost global domain of attraction. This estimation scheme is discretized for computer implementation using discrete variational mechanics, as a first order Lie group variational integrator. The continuous and discrete pose estimation schemes require optical measurements of at least three inertially fixed landmarks or beacons in order to estimate instantaneous pose. The discrete estimation scheme can also estimate velocities from such optical measurements. Moreover, all states can be estimated during time periods when measurements of only two inertial vectors, the angular velocity vector, and one feature point position vector are available in body frame. In the presence of bounded measurement noise in the vector measurements, numerical simulations show that the estimated states converge to a bounded neighborhood of the actual states.

Dynamic Analysis of the influence of the Rotor Centroid Offset of Gyrowheel

“…Using the above description, in order to derive the mathematical description of the gyrowheel model with centroid offset, the absolute velocity and acceleration of M body, G body and R body are required, firstly; Secondly, it is necessary to establish the moment balance equation by the D'Alembert principle [6].…”

“…Taking the equation ( 5), equation (6) and equation (7) into equation (8), the following equation can be deduced: , , , ,…”

GPS-denied relative motion estimation for fixed-wing UAV using the variational pose estimator

“…The proof is presented in [12], [22]. In the proposed approach, the time evolution of (ĝ,ξ) has the form of the dynamics of a rigid body with Rayleigh dissipation.…”

Riccati Observer Design for Pose, Linear Velocity and Gravity Direction Estimation Using Landmark Position and IMU Measurements

“…Classical approaches for state estimation are typically based on filtering techniques such as extended Kalman filters (EKF), unscented Kalman filters or particle filters. However, nonlinear observers have increasingly become an alternative to these classical techniques, starting with the work of Salcudean on attitude observer [20] and subsequent contributions by other researchers [1], [5], [6], [8], [10], [15]- [19]. Full pose observer design has recently attracted some particular attention [2]- [4], [9], [11], [13]- [15], [21]- [23].…”

“…Gravity estimation error given by arccos(γ γ) versus time(s) given by ξ r =[15 cos(αt), sin(αt) − 10,15 2 sin(αt) + 10 √ 3] (m), with α = π/10. Due to aerodynamic and centripetal forces acting on the vehicle, its attitude and linear acceleration vary quickly and in large proportions.…”