Iterated invariant Kalman filtering for articulated rigid-body systems: from theoretical foundations to inertial motion estimation for robotics

The invariant extended Kalman filter (IEKF) is known for its strong convergence and consistency properties. Using an invariant definition of the estimation error, it yields propagation and update Jacobians that are independent of the current state for systems with group-affine dynamics and measurements written in invariant form. Although it is a state-of-the-art method for single-body pose estimation, its use in articulated rigid-body systems remains limited. Kinematic constraints, coupled poses, and nontrivial state representations hinder a direct extension of the theory. This thesis addresses this gap by extending invariant Kalman filtering to inertial pose estimation in articulated rigid-body systems.

This thesis makes three main contributions. First, it revisits the treatment of state equality constraints in Kalman filtering. By modeling such constraints as noise-free pseudo-measurements, it formalizes the properties a stochastic filter should satisfy when enforcing exact information, and highlights the challenges of doing so. Second, it introduces the iterated invariant extended Kalman filter (IterIEKF), an iterative variant of the IEKF that systematically improves the accuracy of its update through Gauss–Newton relinearization. In the noise-free case, the IterIEKF comes with theoretical guarantees: it is able to incorporate perfect information consistently and globally on the matrix Lie group state space, which the classical IEKF cannot ensure. Third, this thesis introduces the relative L-extended pose, a matrix Lie group representation of multibody pose. This representation yields group-affine dynamics when each body is equipped with an inertial measurement unit (IMU), and it allows a broad class of joint constraints to be expressed in invariant form. Combined with the IterIEKF, it provides a principled way to handle kinematic constraints within the invariant framework and to transfer the properties of invariant filtering to multibody pose estimation problems.

The proposed framework is validated on two real-world experiments: inertial pose estimation of a UR5e robot during a pick-and-place task and of a human leg during forward lunges. In both cases, the IterIEKF combined with the relative L-extended pose exhibits faster convergence and substantially reduced estimation variance compared with EKF and iterated EKF (IterEKF) baselines, as well as invariant filtering based on a free-segment multibody representation. Overall, this thesis provides a principled route from invariant filtering theory to practical inertial motion estimation for articulated rigid-body systems.