Estimating vector magnitude from its direction and derivative, with application to bearing-only SLAM filter problem

Abstract

For some problems, such as monocular visual odometry (VO), vector measurements are given with unknown magnitude. In VO, the magnitude can be found by recognizing features with known position, or with an extra sensor such as an altimeter. This article presents a nonlinear observer that uses the derivative of the vector as an additional measurement for estimating the magnitude of a vector. For the VO example, this means that the velocity can be estimated by fusing the normalized velocity vector with acceleration measurements. The observer exploits the fact that the dynamics of the normalized vector is dependent on the magnitude of the vector. The observer employs methods from nonlinear/adaptive estimation; filters the unit vector on the unit sphere, and retrieves the magnitude of the vector. The observer is shown to be uniformly semi-globally asymptotically (USGAS) stable and uniformly exponentially stable (UES) in a defined region. The observer is applied to the bearing-only SLAM filter problem as an example.