Landmark Position and Full Pose Estimation of a Rigid Body Using Bearing Measurements

This paper studies the method of estimating the landmark positions as well as the full pose of a rigid body, including the velocity, location, and attitude. First, we propose a linear time-varying (LTV) observer to estimate the states in body frame (landmark locations, gravity, and inertial vector) using an inertial measurement unit (IMU), a monocular camera, and a magnetometer. Thereafter, the landmark position and the full pose are reconstructed in an attitude estimation module and a location reconstruction module. Our observer is proved to be uniformly observable (UO) under a persistency of excitation (PE) condition and uniformly ultimately bounded (UUB) under bounded perturbation. Sufficient simulations are completed to indicate the capability of our proposed scheme.