On-Manifold Preintegration for Real-Time Visual-Inertial Odometry

Current approaches for visual-inertial odometry (VIO) are able to attain highly accurate state estimation via nonlinear optimization. However, real-time optimization quickly becomes infeasible as the trajectory grows over time, this problem is further emphasized by the fact that inertial measurements come at high rate, hence leading to fast growth of the number of variables in the optimization. In this paper, we address this issue by preintegrating inertial measurements between selected keyframes into single relative motion constraints. Our first contribution is a \emph{preintegration theory} that properly addresses the manifold structure of the rotation group. We formally discuss the generative measurement model as well as the nature of the rotation noise and derive the expression for the \emph{maximum a posteriori} state estimator. Our theoretical development enables the computation of all necessary Jacobians for the optimization and a-posteriori bias correction in analytic form. The second contribution is to show that the preintegrated IMU model can be seamlessly integrated into a visual-inertial pipeline under the unifying framework of factor graphs. This enables the application of incremental-smoothing algorithms and the use of a \emph{structureless} model for visual measurements, which avoids optimizing over the 3D points, further accelerating the computation. We perform an extensive evaluation of our monocular \VIO pipeline on real and simulated datasets. The results confirm that our modelling effort leads to accurate state estimation in real-time, outperforming state-of-the-art approaches.Comment: 20 pages, 24 figures, accepted for publication in IEEE Transactions on Robotics (TRO) 201

Extended Kalman Filter on SE(3) for Geometric Control of a Quadrotor UAV

An extended Kalman filter (EKF) is developed on the special Euclidean group, SE(3) for geometric control of a quadrotor UAV. It is obtained by performing an extensive linearization on SE(3) to estimate the state of the quadrotor from noisy measurements. Proposed estimator considers all the coupling effects between rotational and translational dynamics, and it is developed in a coordinate-free fashion. The desirable features of the proposed EKF are illustrated by numerical examples and experimental results for several scenarios. The proposed estimation scheme on SE(3) has been unprecedented and these results can be particularly useful for aggressive maneuvers in GPS denied environments or in situations where parts of onboard sensors fail.Comment: arXiv admin note: text overlap with arXiv:1304.6765, arXiv:1411.298

Lie Algebraic Unscented Kalman Filter for Pose Estimation

An unscented Kalman filter for matrix Lie groups is proposed where the time propagation of the state is formulated on the Lie algebra. This is done with the kinematic differential equation of the logarithm, where the inverse of the right Jacobian is used. The sigma points can then be expressed as logarithms in vector form, and time propagation of the sigma points and the computation of the mean and the covariance can be done on the Lie algebra. The resulting formulation is to a large extent based on logarithms in vector form, and is therefore closer to the UKF for systems in $\mathbb{R}^n$. This gives an elegant and well-structured formulation which provides additional insight into the problem, and which is computationally efficient. The proposed method is in particular formulated and investigated on the matrix Lie group $SE(3)$. A discussion on right and left Jacobians is included, and a novel closed form solution for the inverse of the right Jacobian on $SE(3)$ is derived, which gives a compact representation involving fewer matrix operations. The proposed method is validated in simulations

Observer design on the Special Euclidean group SE

Abstract— This paper proposes a nonlinear pose observer designed directly on the Lie group structure of the Special Euclidean group SE(3). We use a gradient-based observer design approach and ensure that the derived observer innovation can be implemented from position measurements. We prove local exponential stability of the error and instability of the non-zero critical points. Simulations indicate that the observer is indeed almost globally stable as would be expected