Stable embedding based Kalman filters for state estimation of systems defined on manifolds

Abstract

In this thesis, we propose a new filtering paradigm of dealing with systems on manifolds by using the mathematical theory called stable embedding. We extend a given system on a manifold to an ambient open set in Euclidean space and modify the system such that the extended system is stable on the manifold. Then, we apply the Kalman filters derived in Euclidean space to the modified dynamics. This method has the merit that we can apply various Kalman filters derived in Euclidean space including extended Kalman filters (EKF) for state estimation of systems defined on manifolds. First, we combine stable embedding with EKF (EKF-SE) and analyze the performance. EKF-SE is successfully applied to the rigid body attitude dynamics whose configuration space is the special orthogonal group. EKF-SE is shown to be efficient in terms of computation and accurate in comparison with the standard EKF. Second, we combine stable embedding with unscented Kalman filter (UKF-SE) and analyze the performance. We confirm the performance of UKF-SE by applying it to the satellite system model and comparing the performance with other UKFs devised specifically for systems on manifolds. UKF-SE has the lowest estimation error, keeps the state estimates in close proximity to the manifold as expected, and consumes a minor amount of computation time. Finally, we apply EKF-SE to visual inertial odometry (VIO). VIO has been one of the primary research areas in robotics because it enables navigation in GPS denied environments by combining the inertial measurement unit and camera measurements to estimate the poses of sensor platforms. We apply stable embedding to VIO with stereo camera and test this VIO algorithm on real-world micro aerial vehicle flight dataset. Our VIO shows the best performance among the compared VIOs with the trajectory error and the computational complexity taken into account.