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.

Globally stable velocity estimation using normalized velocity measurement

“…The last observer was originally presented in Bjørne et al (2018), with proof of semi-global asymptotic stability and locally exponential stability. The observer utilizes the time derivative of the normalized vector and how it relates to the magnitude of the vector corresponding to the unit vector measurement.…”

“…An observer able to estimate velocity with a similar sensor setup as in Grabe et al (2015) was presented in Bjørne et al (2018). The main difference is that a normalized velocity is assumed to come from the camera instead of a homography transform.…”

“…Thus, globally stable velocity estimation with a defined PE condition, and without the need to observe features for several images is possible without the need to observe a flat surface. The presented article builds on Bjørne et al (2018), where we compared the novel observer with other similar observers, and how they perform when used for the ego-motion estimation.…”

“…There are several observers able to perform this estimation, since the ego-motion estimation with normalized velocity measurement is mathematically identical to the problem of estimating the distance to a landmark assuming velocity and bearing measurements are available. This implies that the ego motion estimation assuming normalized velocity measurements can be performed by several observers, that originally were designed for the latter problem (Bjørne et al, 2018; Hamel and Samson, 2017; Le Bras et al, 2017; Lourenço et al, 2018). In addition, the observers will have the same stability results.…”

“…The different observers are compared qualitatively, in simulations and with experimental data. It is also shown how the novel observer presented in Bjørne et al (2018), can tune its convergence rate. They are also compared with an EKF and the homography-based observer (Grabe et al, 2015).…”