Structure-Preserving Unscented Kalman Filter for Planar Mobile Robots

Constrained robots play an important role in industrial processes and delivery tasks. To improve the numerical accuracy of robot state estimation, many filtering methods including the well-known unscented Kalman filter (UKF) have been widely studied. However, most conventional propagation schemes in UKF are based on a direct discretization of the continuous-time equations, which suffer from the problem of ignoring physical structures and properties of a robot (such as physical constraints, energy conservation, and manifold structure preservation) due to the numerical dissipation of the time discretization scheme. In this letter, we introduce a structure-preserving unscented Kalman filter (SP-UKF) for mobile robots. By using differential geometry, the resulting time propagation step of the proposed filter shows the benefit of preserving the no-slip constraint of a mobile robot and at the same time respecting key structures and physical laws of the system. Numerical results validate the efficiency and the structure-preserving properties of the proposed approach.