Pose Tracking with Iterative Extended Kalman Filter
Tracking ego pose is critical in autonomous driving. In this article, we will discuss how to fuse the IMU, wheel encoder, GPS, etc. to track the ego pose. We will derive the pose tracking algorithm based on the iterative extended Kalman filter. This document mainly follows [1] and [2]. Preliminaries Let $\mathcal{M}$ be the manifold of dimension $n$ in consideration (e.g., $\mathcal{M} = SO(3)$). Since manifolds are locally homeomorphic to $\mathbb{R}^n$, we can establish a bijective mapping from a local neighborhood on $\mathcal{M}$ to its tangent space $\mathbb{R}^n$ via two encapsulation operators $\boxplus$ and $\boxminus$: ...