strapdown inertial navigation basics with an explanation of IMU pre-integration
Inertial Measurement Units (IMU) are typically used to track the position and orientation of an object body relative to a known starting position, orientation and velocity. Two configurations of inertial navigation are common
These devices typically contain
In this blog, I cover the basics required to understand inertial navigation in robot state estimation, and also explain the motivation behind IMU pre-integration
An expert reader may choose to skip this section, however this section is important to understand the underlying math and motivation behind IMU pre-integration. This section is largely adapted from Sola et al.
is a curved and differentiable (no-spikes / edges) hyper-surface embedded in a higher dimension that locally resembles a linear space \(\mathbb{R}^n\). Examples:
is a set with a composition operator \(\circ\) that for elements \(\mathcal{X}, \mathcal{Y}, \mathcal{Z} \in \mathcal{G}\) satisfies the following axioms:
A Lie Group is a smooth manifold meaning that it has a locally euclidean structure, and also satisfies the properties of a group. A Lie Grou has identical curvature and other properties at every point on the manifold (imagine a circle for example). Some examples are:
Note that the 2-sphere \(\text{S}^2\) is not a Lie Group, since we cannot define a group composition operator over it. To understand this, let us consider the hairy ball theorem
Elements of the Lie Group can act on elements from other sets. For example, a unit quaternion \(\mathbf{q} \in \mathbb{H}\) acts on an element \(\mathbf{x} \in \mathbb{R}^3\) through quaternion multiplication to cause its rotation \(\mathbb{H} \times \mathbb{R}^3 \rightarrow \mathbb{R}^3 : \mathbf{q} \cdot \mathbf{x} \cdot \mathbf{q}^*\).
Let \(\mathcal{X}(t)\) be a point on the Lie manifold \(\mathcal{M}\), then taking its time derivative we obtain \(\dot{\mathcal{X}} = \frac{d\mathcal{X}}{dt}\) that belongs to its tangent space at \(\mathcal{X}\) (or roughly linearized at \(\mathcal{X}\)) denoted as \(T_\mathcal{X}\mathcal{M}\). Since we note that the lie group has the same curvature throughout the manifold, the tangent space \(T_{\mathcal{X}}\mathcal{M}\) also has the same structure everywhere. In fact, by definition every Lie group of dimension \(n\) must have a tangent space described by \(n\) basis elements \(\{\text{E}_1 \dots \text{E}_n\}\) (sometimes also called generators) for \(T_\mathcal{X}\mathcal{M}\).
For instance, the tangent space for the group of unit complex numbers is the tangent to a circle at any point forming a straight line.
The Lie Algebra then is simply the tangent space of a Lie group – linearized – at the identity element \(\mathcal{E}\) of the group. Every Lie group \(\mathcal{M}\) has an associated lie algebra \(\mathfrak{m} \triangleq T_\mathcal{E}\mathcal{M}\). The Lie algebra \(\mathfrak{m}\) is a vector space.
Now, we may define two operators to navigate the Lie group as follows:
The \(\text{exp}\) naturally arises when considering the time derivative, or an infinitesimal tangent increment \(v \in T_\mathcal{X}\mathcal{M}\) per unit time on the group manifold:
\(\begin{align} \frac{d\mathcal{X}}{dt} &= \mathcal{X}{v} \\ \frac{d\mathcal{X}}{\mathcal{X}} &= v~dt \\ \text{integrating} \implies \mathcal{X}(t) &= \mathcal{X}(0) \text{exp}(vt) \\ \implies \text{exp}(vt) &= \mathcal{X}(0)^{-1}\mathcal{X}(t) \in \mathcal{M} \end{align}\) i.e., \(\text{exp}(vt)\) is a group element.
The group of rotations \(\mathbf{SO}(3)\) is a matrix group of size 9 operating on \(\mathbb{R}^3\), with the following constraints:
For this group, we have the special orthogonality condition which can be written as:
\[\mathbf{R}^{-1}\mathbf{R} = \mathbf{I} = \mathbf{R}^\top \mathbf{R}\]since \(\mathbf{R}^{-1} = \mathbf{R}^\top\). Now, to obtain the tangent space for this group, let’s take the time differential of this equation:
\[\begin{align} \dot{\mathbf{R}}^\top \mathbf{R} + \mathbf{R}\dot{\mathbf{R}}^\top &= 0 \\ \implies \dot{\mathbf{R}}^\top \mathbf{R} &= -\mathbf{R}\dot{\mathbf{R}}^\top \\ \implies \dot{\mathbf{R}}^\top \mathbf{R} &= - (\dot{\mathbf{R}}^\top \mathbf{R})^\top \end{align}\]This means that \(\dot{\mathbf{R}}^\top \mathbf{R}\) is skew-symmetric, and skew symmetric matrices always have the following form:
\[[\boldsymbol\omega]_\times = \begin{bmatrix}0 & -\omega_z & \omega_y \\ \omega_z & 0 & -\omega_x \\ -\omega_y & \omega_x & 0\end{bmatrix}\]Therefore we can write \(\dot{\mathbf{R}}^\top \mathbf{R}\) is of the form \([\boldsymbol\omega]_\times\) or
\(\begin{align} \dot{\mathbf{R}} = \mathbf{R}[\boldsymbol\omega]_\times \label{eq:so3_lie_algebra} \end{align}\).
When \(\mathbf{R} = \mathbf{I}\), then \(\dot{\mathbf{R}} = [\boldsymbol\omega]_\times\), which consequently means that \([\boldsymbol\omega]_\times\) the space of skew symmetric matrices forms the Lie algebra for \(\text{SO}(3)\). Finally, we observe that \([\boldsymbol\omega]_\times\) is 3 degrees of freedom by inspection, and that it can be represented as a linear combination of generators as follows:
\[\begin{align} [\boldsymbol\omega]_\times = \omega_x \begin{bmatrix}0 & 0 & 0 \\ 0 & 0 & -1 \\ 0 & 1 & 0\end{bmatrix} + \omega_y\begin{bmatrix} 0 & 0 & 1 \\ 0 & 0 & 0 \\ -1 & 0 & 0\end{bmatrix} + \omega_z \begin{bmatrix}0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 0\end{bmatrix} \label{eq:so3_basis} \end{align}\]if we denote these basis elements as \(\text{E}_x, \text{E}_y, \text{E}_z\) respectively, then we can denote \(\boldsymbol{\omega} = (\omega_x, \omega_y, \omega_z) \in \mathbb{R}^3\) as the vector representation of the lie algebra.
Now, let us attempt to obtain a closed form expression for the \(\text{exp}\) map of \(\text{SO}(3)\). We see from equation (\(\ref{eq:so3_lie_algebra}\)), that we have a differential equation, where \(\dot{\mathbf{R}} = \mathbf{R} [\boldsymbol{\omega}]_\times \in T_\mathbf{R}\text{SO}(3)\). For infinitesimal time increments \(\Delta t\), we can assume that \(\omega\) is constant, then we obtain the solution to the ordinary differential equation as:
\[\begin{align} \int \dot{\mathbf{R}} &= \int \mathbf{R}[\boldsymbol\omega]_\times \Delta t \\ \implies \mathbf{R}(t) &= \mathbf{R}_{0} \text{exp}([\boldsymbol\omega]_\times \Delta t) \end{align}\]If we start at the origin \(\mathbf{R}_0 = \mathbf{I}\), then we have \(\mathbf{R}(t) = \text{exp}([\boldsymbol{\omega}]_\times \Delta t)\).
Now since \(\boldsymbol\omega\) can also be represented as a vector element (see equation (\(\ref{eq:so3_basis}\))), we can define \(\boldsymbol{\theta} \triangleq \omega \Delta t = \mathbf{u} \theta \in \mathbb{R}^3\), where \(\mathbf{u}\) is a unit vector denoting the axis of rotation, and \(\boldsymbol\theta\) denotes the rotation about said axis. It must be noted that 3-axis gyroscopes measure this angular velocity \(\omega\).
Let us now expand the matrix exponential terms:
\[\begin{align} \mathbf{R} = \text{exp}([\boldsymbol\theta]_\times) = \sum_k \frac{\theta^k}{k !}([\mathbf{u}]_\times)^k \end{align}\]Using the properties of skew symmetric matrices we see that \([\mathbf{u}]^0_\times = \mathbf{I}, [\mathbf{u}]^3_\times = -[\mathbf{u}]_\times, [\mathbf{u}]^3_\times = -[\mathbf{u}]^2_\times\). We can thus rewrite the series above as
\[\begin{align} \mathbf{R} &= \mathbf{I} + [\mathbf{u}]_\times \Bigl\{ \theta - \frac{1}{3!}\theta^3 + \frac{1}{5!}\theta^5 - \dots \Bigr\} + [\mathbf{u}]_\times^2 \Bigl\{ \frac{1}{2} \theta^2 - \frac{1}{4!}\theta^4 + \frac{1}{6!}\theta^6 -\dots \Bigr\} \\ \mathbf{R} &= \mathbf{I} + [\mathbf{u}]_\times \sin \theta + [\mathbf{u}]^2_\times (1 - \cos \theta) \label{eq:rodrigues} \end{align}\]Where equation (\(\ref{eq:rodrigues}\)) is the closed form expression for the exponential map for \(\text{SO}(3)\) and also known as the rodrigues formula
As eluded to in the Introduction, an ideal Inertial Measurement Unit (IMU) mainly contains two sensors Gyroscope and Accelerometer and sometimes optionally a magnetometer.
An ideal Gyroscope measures the angular velocity or the angular rate of motion \({}_b\omega_k\) in the body frame \(b\) at time instant \(k\).
Let us consider the IMU in Figure 1. As it travels along the trajectory from \(i\) to \(j\), the axis of rotation changes continuously. If we make the piece-wise linear approximation and assume that the axis of rotation remains fixed between two timesteps, then for an angular velocity measurement at \(k\) as \(\omega_k\), angular change in rotation is \(\omega_k \Delta t_k^{k+1} \in \mathfrak{so}(3)\). Subsequently \(\Delta\mathbf{R}_k^{k+1} = \text{Exp}(\omega_k \Delta t_k^{k+1})\).
Now assuming that all \(\Delta t\) are equal we have for the complete trajectory:
\[\begin{align} {}_W \mathbf{R}_j &= {}_W \mathbf{R}_i \text{Exp}(\omega_i \Delta t_i) \dots \text{Exp}(\omega_k \Delta t_k)\dots \text{Exp}(\omega_j \Delta t_j) \\ \implies {}_W \mathbf{R}_j &= {}_W \mathbf{R}_i \prod_{k=i}^{j}\text{Exp}(\omega_k \Delta t_k) \end{align}\]If you found this useful, please cite this as:
Sharma, Akash (Jul 2024). almost everything about IMU navigation. https://akashsharma02.github.io.
or as a BibTeX entry:
@article{sharma2024almost-everything-about-imu-navigation,
title = {almost everything about IMU navigation},
author = {Sharma, Akash},
year = {2024},
month = {Jul},
url = {https://akashsharma02.github.io/blog/2024/imu-navigation/}
}