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*} \boldsymbol{\omega}_m(t) &= \boldsymbol{\omega}(t) + \mathbf{b}_{g}(t) + \mathbf{n}_{{g}}(t)\\ \mathbf{a}_m(t) &= \mathbf{a}(t) + {}^I_G\mathbf{R}(t) {^G\mathbf{g}} + \mathbf{b}_{a}(t) + \mathbf{n}_{{a}}(t) \end{align*}

where $\boldsymbol{\omega}$ and $\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.

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}_{\mathbf{g}}(t) \\ \mathbf{b}_{\mathbf{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 Kinematics

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

\begin{align*} \newcommand{\comm}[1]{} ^I_G\dot{\bar{q}}(t) \comm{ &= \lim_{\delta t \to 0} \frac{1}{\delta t} (^{I_{t + \delta t}}_G\bar{q} - \text{}^{I_{t}}_G\bar{q})\\ &= \lim_{\delta t \to 0} \frac{1}{\delta t} (^{I_{t + \delta t}}_{L_{t}}\bar{q} \otimes ^{I_{t}}_{G}\bar{q} - \bar{q}_0 \otimes \text{}^{I_{t}}_G\bar{q})\\ &= \lim_{\delta t \to 0} \frac{1}{\delta t} (^{I_{t + \delta t}}_{L_{t}}\bar{q} - \bar{q}_0 ) \otimes \text{}^{I_{t}}_{G}\bar{q}\\ &\approx \lim_{\delta t \to 0} \frac{1}{\delta t} \Bigg (\begin{bmatrix} \frac{1}{2}\delta \boldsymbol{\theta}\\ 1 \end{bmatrix} -\begin{bmatrix} \boldsymbol{0}\\ 1 \end{bmatrix} \Bigg ) \otimes \text{}^{I_{t}}_{G}\bar{q} \\ &= \frac{1}{2} \begin{bmatrix} \boldsymbol{\omega}\\ 0 \end{bmatrix} \otimes \text{}^{I_{t}}_{G}\bar{q}\\} &= \frac{1}{2} \begin{bmatrix} -\lfloor \boldsymbol{\omega}(t) \times \rfloor && \boldsymbol{\omega}(t) \\ -\boldsymbol{\omega}^\top(t) && 0 \end{bmatrix} \text{}^{I_{t}}_{G}\bar{q}\\ &=: \frac{1}{2} \boldsymbol{\Omega}(\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\mathbf{a}(t) \\ \dot{\mathbf{b}}_{\mathbf{g}}(t) &= \mathbf{n}_{wg}\\ \dot{\mathbf{b}}_{\mathbf{a}}(t) &= \mathbf{n}_{wa} \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.

Continuous-time IMU Propagation

Given the continuous-time measurements $\boldsymbol{\omega}_m(t)$ and $\mathbf{a}_m(t)$ in the time interval $t \in [t_k,t_{k+1}]$ , and their estimates, i.e. after taking the expectation, $\hat{\boldsymbol{\omega}}(t) = \boldsymbol{\omega}_m(t) - \hat{\boldsymbol{b}}_g(t)$ and $\hat{\boldsymbol{a}}(t) = \boldsymbol{a}_m(t) - \hat{\boldsymbol{b}}_a(t) - ^I_G\hat{\mathbf{R}}(t){}^G\mathbf{g}$ , we can define the solutions to the above IMU kinematics differential equation. The solution to the quaternion evolution has the following general form:

\begin{align*} ^{I_{t}}_{G}\bar{q} = \boldsymbol{\Theta}(t,t_k) \text{}^{I_{k}}_{G}\bar{q} \end{align*}

Differentiating and reordering the terms yields the governing equation for $\boldsymbol{\Theta}(t,t_k)$ as

\begin{align*} \newcommand{\comm}[1]{} \boldsymbol{\Theta}(t,t_k) &= \text{}^{I_t}_G\bar{q} \text{ }^{I_k}_{G}\bar{q}^{-1}\\ \Rightarrow \dot{\boldsymbol{\Theta}}(t,t_k) &= \text{}^{I_t}_G\dot{\bar{q}} \text{ }^{I_k}_{G}\bar{q}^{-1}\\ &= \frac{1}{2} \boldsymbol{\Omega}(\boldsymbol{\omega}(t)) \text{ }^{I_t}_{G}\bar{q} \text{ }^{I_k}_{G}\bar{q}^{-1}\\ &= \frac{1}{2} \boldsymbol{\Omega}(\boldsymbol{\omega}(t)) \boldsymbol{\Theta}(t,t_k) \end{align*}

with $\boldsymbol{\Theta}(t_k,t_k) = \mathbf{I}_{4}$ . If we take $\boldsymbol{\omega}(t) = \boldsymbol{\omega}$ to be constant over the the period $\Delta t = t_{k+1} - t_k$ , then the above system is linear time-invarying (LTI), and $\boldsymbol{\Theta}$ can be solved as (see [Stochastic Models, Estimation, and Control] [12]):

\begin{align*} \boldsymbol{\Theta}(t_{k+1},t_k) &= \exp\bigg(\frac{1}{2}\boldsymbol{\Omega}(\boldsymbol{\omega})\Delta t\bigg)\\ &= \cos\bigg(\frac{|\boldsymbol{\omega}|}{2} \Delta t \bigg) \cdot \mathbf{I}_4 + \frac{1}{|\boldsymbol{\omega}|}\sin\bigg(\frac{|\boldsymbol{\omega}|}{2} \Delta t \bigg) \cdot \boldsymbol{\Omega}(\boldsymbol{\omega})\\ &\simeq \mathbf{I}_4 + \frac{\Delta t}{2}\boldsymbol{\Omega}(\boldsymbol{\omega}) \end{align*}

where the approximation assumes small $|\boldsymbol{\omega}|$ . We can formulate the quaternion propagation from $t_k$ to $t_{k+1}$ using the estimated rotational velocity $\hat{\boldsymbol{\omega}}(t) = \hat{\boldsymbol{\omega}}$ as:

\begin{align*} \text{}^{I_{k+1}}_{G}\hat{\bar{q}} = \exp\bigg(\frac{1}{2}\boldsymbol{\Omega}(\hat{\boldsymbol{\omega}})\Delta t\bigg) \text{}^{I_{k}}_{G}\hat{\bar{q}} \end{align*}

Having defined the integration of the orientation, we can integrate the velocity and position over the measurement interval:

\begin{align*} ^G\hat{\mathbf{v}}_{k+1} &= \text{}^G\hat{\mathbf{v}}_{I_k} + \int_{t_{k}}^{t_{k+1}} {^G\hat{\mathbf{a}}(\tau)} d\tau \\ &= \text{}^G\hat{\mathbf{v}}_{I_k} - {}^G\mathbf{g}\Delta t+ \int_{t_{k}}^{t_{k+1}} {^G_{I_{\tau}}\hat{\mathbf{R}}(\mathbf{a}_m(\tau) - \hat{\mathbf{b}}_{\mathbf{a}}(\tau)) d\tau}\\[1em] ^G\hat{\mathbf{p}}_{I_{k+1}} &= \text{}^G\hat{\mathbf{p}}_{I_k} + \int_{t_{k}}^{t_{k+1}} {^G\hat{\mathbf{v}}_I(\tau)} d\tau \\ &= \text{}^G\hat{\mathbf{p}}_{I_k} + \text{}^G\hat{\mathbf{v}}_{I_k} \Delta t - \frac{1}{2}{}^G\mathbf{g}\Delta t^2 + \int_{t_{k}}^{t_{k+1}} \int_{t_{k}}^{s} {^G_{I_{\tau}}\hat{\mathbf{R}}(\mathbf{a}_m(\tau) - \hat{\mathbf{b}}_{\mathbf{a}}(\tau)) d\tau ds} \end{align*}

Propagation of each bias $\hat{\mathbf{b}}_{\mathbf{g}}$ and $\hat{\mathbf{b}}_{\mathbf{a}}$ is given by:

\begin{align*} \hat{\mathbf{b}}_{\mathbf{g},k+1} &= \hat{\mathbf{b}}_{\mathbf{g},k} + \int_{t_{k+1}}^{t_{k}} {\hat{\mathbf{n}}_{wg}(\tau)} d\tau \\ &= \hat{\mathbf{b}}_{\mathbf{g},k} \\ \hat{\mathbf{b}}_{\mathbf{a},k+1} &= \hat{\mathbf{b}}_{\mathbf{a},k} + \int_{t_{k+1}}^{t_{k}} {\hat{\mathbf{n}}_{wa}(\tau)} d\tau \\ &= \hat{\mathbf{b}}_{\mathbf{a},k} \end{align*}

The biases will not evolve since our random walk noises $\hat{\mathbf{n}}_{wg}$ and $\hat{\mathbf{n}}_{wa}$ are zero-mean white Gaussian. All of the above integrals could be analytically or numerically solved if one wishes to use the continuous-time measurement evolution model.

Discrete-time IMU Propagation

A simpler method is to model the measurements as discrete-time over the integration period. To do this, the measurements can be assumed to be constant during the sampling period. We employ this assumption and approximate that the measurement at time $t_k$ remains the same until we get the next measurement at $t_{k+1}$ . For the quaternion propagation, it is the same as continuous-time propagation with constant measurement assumption ${\boldsymbol{\omega}}_{m}(t_k) = {\boldsymbol{\omega}}_{m,k}$ . We use subscript $k$ to denote it is the measurement we get at time $t_k$ . Therefore the propagation of quaternion can be written as:

\begin{align*} \text{}^{I_{k+1}}_{G}\hat{\bar{q}} = \exp\bigg(\frac{1}{2}\boldsymbol{\Omega}\big({\boldsymbol{\omega}}_{m,k}-\hat{\mathbf{b}}_{g,k}\big)\Delta t\bigg) \text{}^{I_{k}}_{G}\hat{\bar{q}} \end{align*}

For the velocity and position propagation we have constant $\mathbf{a}_{m}(t_k) = \mathbf{a}_{m,k}$ over $t \in [t_k, t_{k+1}]$ . We can therefore directly solve for the new states as:

\begin{align*} ^G\hat{\mathbf{v}}_{k+1} &= \text{}^G\hat{\mathbf{v}}_{I_k} - {}^G\mathbf{g}\Delta t +\text{}^{I_k}_G\hat{\mathbf{R}}^\top(\mathbf{a}_{m,k} - \hat{\mathbf{b}}_{\mathbf{a},k})\Delta t\\ ^G\hat{\mathbf{p}}_{I_{k+1}} &= \text{}^G\hat{\mathbf{p}}_{I_k} + {}^G\hat{\mathbf{v}}_{I_k} \Delta t - \frac{1}{2}{}^G\mathbf{g}\Delta t^2 + \frac{1}{2} \text{}^{I_k}_{G}\hat{\mathbf{R}}^\top(\mathbf{a}_{m,k} - \hat{\mathbf{b}}_{\mathbf{a},k})\Delta t^2 \end{align*}

The propagation of each bias is likewise the continuous system:

\begin{align*} \hat{\mathbf{b}}_{\mathbf{g},k+1} &= \hat{\mathbf{b}}_{\mathbf{g},k}\\ \hat{\mathbf{b}}_{\mathbf{a},k+1} &= \hat{\mathbf{b}}_{\mathbf{a},k} \end{align*}

Discrete-time Error-state Propagation

In order to propagate the covariance matrix, we should derive the error-state propagation, i.e., computing the system Jacobian $\boldsymbol{\Phi}(t_{k+1},t_k)$ and noise Jacobian $\mathbf{G}_{k}$ . In particular, when the covariance matrix of the continuous-time measurement noises is given by $\mathbf{Q}_c$ , then the discrete-time noise covariance $\mathbf{Q}_d$ can be computed as (see [Indirect Kalman Filter for 3D Attitude Estimation] [19] Eq. (129) and (130)):

\begin{align*} \sigma_{g} &= \frac{1}{\sqrt{\Delta t}}~ \sigma_{g_c} \\ \sigma_{bg} &= \sqrt{\Delta t}~ \sigma_{bg_c} \\[1em] \mathbf{Q}_{meas} &= \begin{bmatrix} \frac{1}{\Delta t}~ \sigma_{g_c}^2~ \mathbf{I}_3 & \mathbf{0}_3 \\ \mathbf{0}_3 & \frac{1}{\Delta t}~ \sigma_{a_c}^2~ \mathbf{I}_3 \end{bmatrix} \\ \mathbf{Q}_{bias} &= \begin{bmatrix} \Delta t~ \sigma_{bg_c}^2~ \mathbf{I}_3 & \mathbf{0}_3 \\ \mathbf{0}_3 & \Delta t~ \sigma_{ba_c}^2~ \mathbf{I}_3 \end{bmatrix} \end{align*}

where $\mathbf{n} = [ \mathbf{n}_g ~ \mathbf{n}_a ~ \mathbf{n}_{bg} ~ \mathbf{n}_{ba} ]^\top$ are the discrete IMU sensor noises which have been converted from their continuous representations. We define the stacked discrete measurement noise as follows:

\begin{align*} \mathbf{Q}_{d} &= \begin{bmatrix} \mathbf{Q}_{meas} & \mathbf{0}_3 \\ \mathbf{0}_3 & \mathbf{Q}_{bias} \end{bmatrix} \end{align*}

The method of computing Jacobians is to "perturb" each variable in the system and see how the old error "perturbation" relates to the new error state. That is, $\boldsymbol{\Phi}(t_{k+1},t_k)$ and $\mathbf{G}_{k}$ can be found by perturbing each variable as:

\begin{align*} \tilde{\mathbf{x}}_I(t_{k+1}) = \boldsymbol{\Phi}(t_{k+1},t_k) \tilde{\mathbf{x}}_I(t_{k}) + \mathbf{G}_{k} \mathbf{n} \end{align*}

For the orientation error propagation, we start with the $\mathbf{SO}(3)$ perturbation using ${}^{I}_G \mathbf{R} \approx (\mathbf{I}_3 - \lfloor ^{I}_{G}\tilde{\boldsymbol{\theta}}\times\rfloor)^{I}_{G} \hat{\mathbf{R}}$ :

\begin{align*} {}^{I_{k+1}}_G \mathbf{R} &= \text{}^{I_{k+1}}_{I_{k}} \mathbf{R} \text{}^{I_{k}}_G \mathbf{R} \\ (\mathbf{I}_3 - \lfloor ^{I_{k+1}}_{G}\tilde{\boldsymbol{\theta}}\times\rfloor)^{I_{k+1}}_{G} \hat{\mathbf{R}} &\approx \textrm{exp}(-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t - {}^{I_{k}}\tilde{\boldsymbol{\omega}}\Delta t) (\mathbf{I}_3 - \lfloor ^{I_{k}}_{G}\tilde{\boldsymbol{\theta}}\times\rfloor)^{I_{k}}_{G} \hat{\mathbf{R}}\\ &=\textrm{exp}(-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t)\textrm{exp}(-\mathbf{J}_r(-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t){}^{I_{k}}\tilde{\boldsymbol{\omega}}\Delta t) (\mathbf{I}_3 - \lfloor ^{I_{k}}_{G}\tilde{\boldsymbol{\theta}}\times\rfloor)^{I_{k}}_{G} \hat{\mathbf{R}}\\ &=\text{}^{I_{k+1}}_{I_{k}} \hat{\mathbf{R}} (\mathbf{I}_3 - \lfloor \mathbf J_r(-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t) \tilde{\boldsymbol{\omega}}_k\Delta t \times\rfloor) (\mathbf{I}_3 - \lfloor ^{I_k}_{G}\tilde{\boldsymbol{\theta}}\times\rfloor) \text{}^{I_{k}}_G \hat{\mathbf{R}} \end{align*}

where $\tilde{\boldsymbol{\omega}} = \boldsymbol{\omega} - \hat{\boldsymbol{\omega}} = -(\tilde{\mathbf{b}}_{\mathbf{g}} + \mathbf{n}_g)$ handles both the perturbation to the bias and measurement noise. $\mathbf {J}_r(\boldsymbol{\theta})$ is the right Jacobian of $\mathbf{SO}(3)$ that maps the variation of rotation angle in the parameter vector space into the variation in the tangent vector space to the manifold [see ov_core::Jr_so3()]. By neglecting the second order terms from above, we obtain the following orientation error propagation:

\begin{align*} \text{}^{I_{k+1}}_{G}\tilde{\boldsymbol{\theta}} \approx \text{}^{I_{k+1}}_{I_{k}}\hat{\mathbf{R}} \text{}^{I_k}_{G}\tilde{\boldsymbol{\theta}} - \text{}^{I_{k+1}}_{I_{k}}\hat{\mathbf{R}}\mathbf J_r(\text{}^{I_{k+1}}_{I_{k}}\hat{\boldsymbol{\theta}}) \Delta t (\tilde{\mathbf{b}}_{\mathbf{g},k} + \mathbf{n}_{\mathbf{g},k}) \end{align*}

Now we can do error propagation of position and velocity using the same scheme:

\begin{align*} ^G\mathbf{p}_{I_{k+1}} &= \text{}^G\mathbf{p}_{I_k} + \text{}^G\mathbf{v}_{I_k} \Delta t - \frac{1}{2}{}^G\mathbf{g}\Delta t^2 + \frac{1}{2}\text{}^{I_k}_G\mathbf{R}^\top \mathbf{a}_{k}\Delta t^2\\ ^G\hat{\mathbf{p}}_{I_{k+1}} + \text{}^G\tilde{\mathbf{p}}_{I_{k+1}} &\approx \text{}^G\hat{\mathbf{p}}_{I_k} + \text{}^G\tilde{\mathbf{p}}_{I_k} + \text{}^G\hat{\mathbf{v}}_{I_k} \Delta t + \text{}^G\tilde{\mathbf{v}}_{I_k} \Delta t - \frac{1}{2}{}^G\mathbf{g}\Delta t^2\\ &\hspace{4cm} + \frac{1}{2} \text{}^{I_k}_{G}\hat{\mathbf{R}}^\top (\mathbf{I}_3 + \lfloor ^{I_{k}}_{G}\tilde{\boldsymbol{\theta}}\times\rfloor) (\hat{\mathbf{a}}_{k} + \tilde{\mathbf{a}}_{k})\Delta t^2\\ \\ ^G\mathbf{v}_{k+1} &= \text{}^G\mathbf{v}_{I_k} - {}^G\mathbf{g}\Delta t +\text{}^{I_k}_G\mathbf{R}^\top\mathbf{a}_{k}\Delta t\\ ^G\hat{\mathbf{v}}_{k+1} + ^G\tilde{\mathbf{v}}_{k+1} &\approx {}^G\hat{\mathbf{v}}_{I_k} + {}^G\tilde{\mathbf{v}}_{I_k} - {}^G\mathbf{g}\Delta t + \text{}^{I_k}_G\hat{\mathbf{R}}^\top (\mathbf{I}_3 + \lfloor ^{I_{k}}_{G}\tilde{\boldsymbol{\theta}}\times\rfloor) (\hat{\mathbf{a}}_{k} + \tilde{\mathbf{a}}_{k})\Delta t \end{align*}

where $\tilde{\mathbf{a}} = \mathbf{a} - \hat{\mathbf{a}} = - (\tilde{\mathbf{b}}_{\mathbf{a}} + \mathbf{n}_{\mathbf{a}})$ . By neglecting the second order error terms, we obtain the following position and velocity error propagation:

\begin{align*} \text{}^G\tilde{\mathbf{p}}_{I_{k+1}} &= \text{}^G\tilde{\mathbf{p}}_{I_k} + \Delta t \text{}^G\tilde{\mathbf{v}}_{I_k} - \frac{1}{2}\text{}^{I_k}_{G}\hat{\mathbf{R}}^\top \lfloor \hat{\mathbf{a}}_{k} \Delta t^2 \times\rfloor ^{I_{k}}_{G}\tilde{\boldsymbol{\theta}} - \frac{1}{2} \text{}^{I_k}_{G}\hat{\mathbf{R}}^\top \Delta t^2 (\tilde{\mathbf{b}}_{\mathbf{a},k} + \mathbf{n}_{\mathbf{a},k})\\ ^G\tilde{\mathbf{v}}_{k+1} &= \text{}^G\tilde{\mathbf{v}}_{I_k} - \text{}^{I_k}_G\hat{\mathbf{R}}^\top \lfloor \hat{\mathbf{a}}_{k} \Delta t \times\rfloor ^{I_{k}}_{G}\tilde{\boldsymbol{\theta}} - \text{}^{I_k}_G\hat{\mathbf{R}}^\top \Delta t (\tilde{\mathbf{b}}_{\mathbf{a},k} + \mathbf{n}_{\mathbf{a},k}) \end{align*}

The propagation of the two random-walk biases are as follows:

\begin{align*} \mathbf{b}_{\mathbf{g},k+1} &= \mathbf{b}_{\mathbf{g},k} + \mathbf{n}_{wg} \\ \hat{\mathbf{b}}_{\mathbf{g},k+1} + \tilde{\mathbf{b}}_{\mathbf{g},k+1} &= \hat{\mathbf{b}}_{\mathbf{g},k} + \tilde{\mathbf{b}}_{\mathbf{g},k} + \mathbf{n}_{wg} \\ \tilde{\mathbf{b}}_{\mathbf{g},k+1} &= \tilde{\mathbf{b}}_{\mathbf{g},k} + \mathbf{n}_{wg} \\[1em] \mathbf{b}_{\mathbf{a},k+1} &= \mathbf{b}_{\mathbf{a},k} + \mathbf{n}_{wa} \\ \hat{\mathbf{b}}_{\mathbf{a},k+1} + \tilde{\mathbf{b}}_{\mathbf{a},k+1} &= \hat{\mathbf{b}}_{\mathbf{a},k} + \tilde{\mathbf{b}}_{\mathbf{a},k} + \mathbf{n}_{wa} \\ \tilde{\mathbf{b}}_{\mathbf{a},k+1} &= \tilde{\mathbf{b}}_{\mathbf{a},k} + \mathbf{n}_{wa} \end{align*}

By collecting all the perturbation results, we can build $\boldsymbol{\Phi}(t_{k+1},t_k)$ and $\mathbf{G}_{k}$ matrices as:

\begin{align*} \boldsymbol{\Phi}(t_{k+1},t_k) &= \begin{bmatrix} \text{}^{I_{k+1}}_{I_{k}}\hat{\mathbf{R}} & \mathbf{0}_3 & \mathbf{0}_3 & - \text{}^{I_{k+1}}_{I_{k}}\hat{\mathbf{R}}\mathbf J_r(\text{}^{I_{k+1}}_{I_{k}}\hat{\boldsymbol{\theta}}) \Delta t & \mathbf{0}_3 \\ - \frac{1}{2}\text{}^{I_k}_{G}\hat{\mathbf{R}}^\top \lfloor \hat{\mathbf{a}}_{k} \Delta t^2 \times\rfloor & \mathbf{I}_3 & \Delta t \mathbf{I}_3 & \mathbf{0}_3 & - \frac{1}{2} \text{}^{I_k}_{G}\hat{\mathbf{R}}^\top \Delta t^2 \\ - \text{}^{I_k}_G\hat{\mathbf{R}}^\top \lfloor \hat{\mathbf{a}}_{k} \Delta t \times\rfloor & \mathbf{0}_3 & \mathbf{I}_3 & \mathbf{0}_3 & - \text{}^{I_k}_G\hat{\mathbf{R}}^\top \Delta t \\ \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{I}_3 & \mathbf{0}_3 \\ \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{I}_3 \end{bmatrix} \end{align*}
\begin{align*} \mathbf{G}_{k} &= \begin{bmatrix} - \text{}^{I_{k+1}}_{I_{k}}\hat{\mathbf{R}}\mathbf J_r(\text{}^{I_{k+1}}_{I_{k}}\hat{\boldsymbol{\theta}}) \Delta t & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 \\ \mathbf{0}_3 & - \frac{1}{2} \text{}^{I_k}_{G}\hat{\mathbf{R}}^\top \Delta t^2 & \mathbf{0}_3 & \mathbf{0}_3 \\ \mathbf{0}_3 & - \text{}^{I_k}_G\hat{\mathbf{R}}^\top \Delta t & \mathbf{0}_3 & \mathbf{0}_3 \\ \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{I}_3 & \mathbf{0}_3 \\ \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{I}_3 \end{bmatrix} \end{align*}

Now, with the computed $\boldsymbol{\Phi}(t_{k+1},t_k)$ and $\mathbf{G}_{k}$ matrices, we can propagate the covariance from $t_k$ to $t_{k+1}$ :

\begin{align*} \mathbf{P}_{k+1|k} = \boldsymbol{\Phi}(t_{k+1},t_k)\mathbf{P}_{k|k}\boldsymbol{\Phi}(t_{k+1},t_k)^\top + \mathbf{G}_k\mathbf{Q}_d\mathbf{G}_k^\top \end{align*}