  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.
