The Complex-Step Derivative Approximation on Matrix Lie Groups

The complex-step derivative approximation is a numerical differentiation technique that can achieve analytical accuracy, to machine precision, with a single function evaluation. In this letter, the complex-step derivative approximation is extended to be compatible with elements of matrix Lie groups. As with the standard complex-step derivative, the method is still able to achieve analytical accuracy, up to machine precision, with a single function evaluation. Compared to a central-difference scheme, the proposed complex-step approach is shown to have superior accuracy. The approach is applied to two different pose estimation problems, and is able to recover the same results as an analytical method when available.

Invariant Extended Kalman Filtering Using Two Position Receivers for Extended Pose Estimation

This paper considers the use of two position receivers and an inertial measurement unit (IMU) to estimate the position, velocity, and attitude of a rigid body, collectively called extended pose. The measurement model consisting of the position of one receiver and the relative position between the two receivers is left invariant, enabling the use of the invariant extended Kalman filter (IEKF) framework. The IEKF possesses various advantages over the standard multiplicative extended Kalman filter, such as state-estimate-independent Jacobians. Monte Carlo simulations demonstrate that the two-receiver IEKF approach yields improved estimates over a two-receiver multiplicative extended Kalman filter (MEKF) and a single-receiver IEKF approach. An experiment further validates the proposed approach, confirming that the two-receiver IEKF has improved performance over the other filters considered.