IMU Propagation Derivations

Contents

IMU Measurements

We use a 6-axis inertial measurement unit (IMU) to propagate the inertial navigation system (INS), which provides measurements of the local rotational velocity (angular rate) $\boldsymbol{\omega}_m$ and local translational acceleration $\mathbf{a}_m$ :

\begin{align*} {}^I\boldsymbol{\omega}_m(t) &= {}^I\boldsymbol{\omega}(t) + \mathbf{b}_{g}(t) + \mathbf{n}_{{g}}(t)\\ {}^I\mathbf{a}_m(t) &= {}^I\mathbf{a}(t) + {}^I_G\mathbf{R}(t) {^G\mathbf{g}} + \mathbf{b}_{a}(t) + \mathbf{n}_{{a}}(t) \end{align*}

where ${}^I\boldsymbol{\omega}$ and ${}^I\mathbf{a}$ are the true rotational velocity and translational acceleration in the IMU local frame $\{I\}$ , $\mathbf{b}_{g}$ and $\mathbf{b}_{a}$ are the gyroscope and accelerometer biases, and $\mathbf{n}_{{g}}$ $\mathbf{n}_{{a}}$ are white Gaussian noise, ${^G\mathbf{g}} = [ 0 ~~ 0 ~~ 9.81 ]^\top$ is the gravity expressed in the global frame $\{G\}$ (noting that the gravity is slightly different on different locations of the globe), and $^I_G\mathbf{R}$ is the rotation matrix from global to IMU local frame.

A more complete and complex model which also takes into account the intrinsic properties of the IMU can be considered. MEMS IMU typically have non-perfect readings which would invalidate the above equations. We can define additional sensor frames $\{w\}$ and $\{a\}$ which can be related back to the "ideal" inertial frame $\{I\}$ that we wish to estimate. Thus we can define the following:

\begin{align*} {}^w\boldsymbol{\omega}_{m}(t) & = \mathbf{T}_{w} {}^w_I\mathbf{R} {}^I\boldsymbol{\omega}(t) + \mathbf{T}_{g} {}^I\mathbf{a}(t) + \mathbf{b}_g(t) + \mathbf{n}_g(t) \\ {}^a\mathbf{a}_{m}(t) & = \mathbf{T}_a {}^a_I\mathbf{R} ({}^I\mathbf{a}(t) + {}^I_G\mathbf{R}(t) {^G\mathbf{g}}) + \mathbf{b}_a(t) + \mathbf{n}_a(t) \end{align*}

where $\mathbf{T}_{w}$ and $\mathbf{T}_a$ are invertible $3\times3$ matrices which represent the scale imperfection and axis misalignment for $\{w\}$ and $\{a\}$ , respectively. ${}^w_I\mathbf{R}$ and ${}^a_I\mathbf{R}$ denote the rotation from the base "inertial" frame $\{I\}$ to the gyroscope and acceleration sensor frames. $\mathbf{T}_g$ denotes the gravity sensitivity, which represents the effects of gravity to the gyroscope readings due to its inherent inertia. Note that this does not take into account the translation between the gyroscope and accelerometer frames, since it is negligible for most MEMS IMUs (see [29] [38]).

Inertial State Vector

We define our INS state vector $\mathbf{x}_I$ at time $t$ as:

\begin{align*} \mathbf{x}_I(t) = \begin{bmatrix} ^I_G\bar{q}(t) \\ ^G\mathbf{p}_I(t) \\ ^G\mathbf{v}_I(t)\\ \mathbf{b}_{{g}}(t) \\ \mathbf{b}_{{a}}(t) \end{bmatrix} \end{align*}

where $^I_G\bar{q}$ is the unit quaternion representing the rotation global to IMU frame, $^G\mathbf{p}_I$ is the position of IMU in global frame, and $^G\mathbf{v}_I$ is the velocity of IMU in global frame. We will often write time as a subscript of $I$ describing the state of IMU at the time for notation clarity (e.g., $^{I_t}_G\bar{q} = \text{}^I_G\bar{q}(t)$ ). In order to define the IMU error state, the standard additive error definition is employed for the position, velocity, and biases, while we use the quaternion error state $\delta \bar{q}$ with a left quaternion multiplicative error $\otimes$ :

\begin{align*} ^I_G\bar{q} &= \delta \bar{q} \otimes \text{}^I_G \hat{\bar{q}}\\ \delta \bar{q} &= \begin{bmatrix} \hat{\mathbf{k}}\sin(\frac{1}{2}\tilde{\theta})\\ \cos(\frac{1}{2}\tilde{\theta}) \end{bmatrix} \simeq \begin{bmatrix} \frac{1}{2}\tilde{\boldsymbol{\theta}}\\ 1 \end{bmatrix} \end{align*}

where $\hat{\mathbf{k}}$ is the rotation axis and $\tilde{\theta}$ is the rotation angle. For small rotation, the error angle vector is approximated by $\tilde{\boldsymbol{\theta}} = \tilde{\theta}~\hat{\mathbf{k}}$ as the error vector about the three orientation axes. The total IMU error state thus is defined as the following 15x1 (not 16x1) vector:

\begin{align*} \tilde{\mathbf{x}}_I(t) = \begin{bmatrix} ^I_G\tilde{\boldsymbol{\theta}}(t) \\ ^G\tilde{\mathbf{p}}_I(t) \\ ^G\tilde{\mathbf{v}}_I(t)\\ \tilde{\mathbf{b}}_{{g}}(t) \\ \tilde{\mathbf{b}}_{{a}}(t) \end{bmatrix} \end{align*}

IMU Kinematic Equations

The IMU state evolves over time as follows (see Indirect Kalman Filter for 3D Attitude Estimation [40]).

\begin{align*} ^I_G\dot{\bar{q}}(t) &= \frac{1}{2} \begin{bmatrix} -\lfloor {}^{I}\boldsymbol{\omega}(t) \times \rfloor && {}^{I}\boldsymbol{\omega}(t) \\ -{}^{I}\boldsymbol{\omega}(t)^\top && 0 \end{bmatrix} \text{}^{I_{t}}_{G}\bar{q}\\ &\triangleq \frac{1}{2} \boldsymbol{\Omega}({}^{I}\boldsymbol{\omega}(t)) \text{}^{I_{t}}_{G}\bar{q}\\ ^G\dot{\mathbf{p}}_I(t) &=\text{} ^G\mathbf{v}_I(t)\\ ^G\dot{\mathbf{v}}_I(t) &=\text{} ^{I_{t}}_G\mathbf{R}^\top {}^{I}\mathbf{a}(t) \\ \dot{\mathbf{b}}_{\mathbf{g}}(t) &= \mathbf{n}_{wg}(t) \\ \dot{\mathbf{b}}_{\mathbf{a}}(t) &= \mathbf{n}_{wa}(t) \end{align*}

where we have modeled the gyroscope and accelerometer biases as random walk and thus their time derivatives are white Gaussian. Note that the above kinematics have been defined in terms of the true acceleration and angular velocities which can be computed as a function of the sensor measurements and state.

Continuous-time Inertial Integration

Given the continuous-time ${}^{I}\boldsymbol{\omega}(t)$ and ${}^{I}\mathbf{a}(t)$ , in the time interval $t \in [t_k,t_{k+1}]$ , and their estimates, i.e. after taking the expectation, ${}^{I}\hat{\boldsymbol{\omega}}(t)$ and ${}^{I}\hat{\boldsymbol{a}}(t)$ , we can define the solutions to the above IMU kinematics differential equation.

\begin{align*} {}^{I_{k+1}}_{G}\mathbf{R} & = \exp \left(-\int^{t_{k+1}}_{t_k} {}^{I}\boldsymbol{\omega}(t_\tau) ~ d \tau \right) {}^{I_k}_{G}\mathbf{R} \\ &\triangleq \Delta \mathbf{R}_k ~{}^{I_k}_{G}\mathbf{R} = {}^{I_{k+1}}_{I_k}\mathbf{R} ~{}^{I_k}_{G}\mathbf{R} \\ {}^G\mathbf{p}_{I_{k+1}} & = {}^G\mathbf{p}_{I_k} + {}^G\mathbf{v}_{I_k}\Delta t_k + {}^{I_k}_{G}\mathbf{R}^\top \int^{t_{k+1}}_{t_k} \int^{s}_{t_k} {}^{I_k}_{I_\tau}\mathbf{R} {}^{I} \mathbf{a}(t_\tau) ~ d \tau d s - \frac{1}{2}{}^G\mathbf{g}\Delta t^2_k \\ &\triangleq {}^G\mathbf{p}_{I_k} + {}^G\mathbf{v}_{I_k}\Delta t_k + {}^{I_k}_{G}\mathbf{R}^\top \Delta \mathbf{p}_{k} - \frac{1}{2}{}^G\mathbf{g}\Delta t^2_k \\ {}^G\mathbf{v}_{I_{k+1}} & = {}^G\mathbf{v}_{I_k} + {}^{I_k}_{G}\mathbf{R}^\top \int^{t_{k+1}}_{t_k} {}^{I_k}_{I_\tau}\mathbf{R} {}^{I} \mathbf{a}(t_\tau) ~ d \tau - {}^G\mathbf{g}\Delta t_k \\ &\triangleq {}^G\mathbf{v}_{I_k} + {}^{I_k}_{G}\mathbf{R}^\top \Delta \mathbf{v}_{k} - {}^G\mathbf{g}\Delta t_k \\ \mathbf{b}_{g_{k+1}} & = \mathbf{b}_{g_{k}} + \int^{t_{k+1}}_{t_k} \mathbf{n}_{wg}(t_\tau) ~ d \tau \\ \mathbf{b}_{a_{k+1}} & = \mathbf{b}_{a_{k}} + \int^{t_{k+1}}_{t_k} \mathbf{n}_{wa}(t_\tau) ~ d \tau \end{align*}

where $\Delta t_k = t_{k+1} - t_{k}$ , ${}^{I_k}_{I_\tau}\mathbf{R} = \exp(\int^{t_\tau}_{t_k} {}^{I}\boldsymbol{\omega}(t_u) ~ d u ) $ , $\exp(\cdot)$ is the $SO(3)$ matrix exponential [8], and vectors are evaluated at their subscript timesteps (e.g. ${}^G\mathbf{v}_{I_k}={}^G\mathbf{v}_{I}(t_k)$ ). The biases are corrupted by random walk noises ${\mathbf{n}}_{wg}$ and ${\mathbf{n}}_{wa}$ that are zero-mean white Gaussians. We have the following integration components:

\begin{align*} \Delta \mathbf{R}_k & \triangleq {}^{I_{k+1}}_{I_k}\mathbf{R} = \exp \left(-\int^{t_{k+1}}_{t_{k}} {}^{I}\boldsymbol{\omega}(t_\tau) ~ d \tau\right) \\ \Delta \mathbf{p}_{k} & \triangleq \int^{t_{k+1}}_{t_{k}} \int^{s}_{t_{k}} {}^{I_k}_{I_\tau}\mathbf{R} {}^{I} \mathbf{a}(t_\tau) ~ d \tau d s \\ \Delta \mathbf{v}_{k} & \triangleq \int^{t_{k+1}}_{t_{k}} {}^{I_k}_{I_\tau}\mathbf{R} {}^{I} \mathbf{a}(t_\tau) ~ d \tau \end{align*}

Mean and Uncertainty Integration

Having defined the general equations for state evolution, we can now look into how to actually perform these integrations. The following two documents detail two different ways to both evolve the state forward and its covariance. The first is a simple discrete state evolution model without any IMU intrinsics and can be helpful to understand the general flow of derivation. The second is the full continuous-time integration (with constant measurement assumption) of the mean and state covariance.

For general background on how to formulate the evolution of states over time we can make a few recommendations. For classical linear systems we can recommend Maybeck's [31] Volume 1 and Bar-Shalom et al. [2] discussion of estimator consistency. For background on Kalman filtering and propagation with similar notation to ours we can recommend Farrell's [14] aided navigation book. Finally, for the inertial problem formulation and equations Chatfield's [5] fundamental remains foundational.