RigidBodyPoseEstimationbased on the Lagrange-d’AlembertPrinciple⋆

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