Invariant Extended Kalman Filter for State Estimation of a Robot With an IMU on an Inclined Plane

Many robots operate on land, or water surfaces, with limited pitch, roll, and vertical motion. While 2D kinematic equations are a simple approach to model these systems, they fail to capture pitch and roll variations on an uneven terrain, or the fact the terrain is not horizontal. This letter presents a novel model that is a compromise between the full 3D inertial measurement unit (IMU) dynamics, and a simple 2D horizontal projection, by retaining pitch, roll and yaw, while projecting position and velocity onto a (non-horizontal) inclined plane. Remarkably, the obtained model perfectly falls into the full-blown framework of the Invariant Extended Kalman Filter (IEKF) since it verifies log-linear error propagation and state-independent Jacobian properties even when incorporating IMU biases. Numerical tests show improved consistency and accuracy over the Extended Kalman Filter.