Invariant Kalman Filter for Relative Dynamics

Abstract: This paper presents an invariant Kalman filter for estimating the relative trajectories between two dynamic systems. Invariant Kalman filters formulate the estimation error in terms of the group operation, ensuring that the error state does not depend on the current state estimate - a property referred to as state trajectory independence. This is particularly advantageous in extended Kalman filters, as it makes the propagation of the error covariance robust to large estimation errors. In this work, we construct invariant Kalman filters to the trajectory of one system relative to another. Specifically, we show that if the relative dynamics can be described solely by relative state variables, they automatically satisfy state trajectory independence, allowing for the development of an invariant Kalman filter. The corresponding relative invariant Kalman filter is formulated in an abstract fashion and is demonstrated numerically for the attitude dynamics of a rigid body.