Lie Algebraic Unscented Kalman Filter for Pose Estimation
This work addresses pose estimation for robotics or computer vision applications, but it is incremental as it builds on existing unscented Kalman filter methods with specific mathematical improvements.
The authors tackled pose estimation by proposing an unscented Kalman filter on matrix Lie groups, using the Lie algebra for time propagation and deriving a novel closed-form inverse of the right Jacobian on SE(3), resulting in a computationally efficient and elegant formulation validated in simulations.
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.