The state definition and general propagation equations are covered in the IMU Propagation Derivations page. Here we will cover the continuous-time integration of the state dynamics and inclusion of IMU intrinsic parameters. First we will cover how we can propagate our mean forward, afterwhich we cover how to derive the linearized error-state system propagation and approximations used. Key references for analytical inertial integration include:
Indirect Kalman Filter for 3D Attitude Estimation [40] Consistency Analysis and Improvement of Vision-aided Inertial Navigation [18] [17] High-precision, consistent EKF-based visual-inertial odometry [27] [26] Closed-form preintegration methods for graph-based visual-inertial navigation [13] [12] Analytic Combined IMU Integration (ACI^2) for Visual-Inertial Navigation [44] [43] Online Self-Calibration for Visual-Inertial Navigation: Models, Analysis and Degeneracy [46] First we can re-write our IMU measurement models to find the the true (or corrected) angular velocity
${}^I\boldsymbol{\omega}(t)$
and linear acceleration
${}^I\mathbf{a}(t)$
. This is how we will relate incoming measurements to IMU dynamics (see IMU Kinematic Equations ).
\begin{align*} {}^I\boldsymbol{\omega}(t) & = {}^I_{w}\mathbf{R} \mathbf{D}_w \left( {}^w\boldsymbol{\omega}_{m}(t) - \mathbf{T}_{g} {}^I\mathbf{a}(t) - \mathbf{b}_g(t) - \mathbf{n}_g(t) \right) \\ {}^I\mathbf{a}(t) & = {}^I_a\mathbf{R} \mathbf{D}_a \left( {}^a\mathbf{a}_{m}(t) - \mathbf{b}_a(t) - \mathbf{n}_a(t) \right) \end{align*}
where
$\mathbf{D}_w = \mathbf{T}^{-1}_w$
and
$\mathbf{D}_a = \mathbf{T}^{-1}_a$
. Note that here we have factored out the
$- {}^I_G\mathbf{R}(t) {^G\mathbf{g}}$
gravity term (see below state evolution equations). In practice, we calibrate
$\mathbf{D}_a$
,
$\mathbf{D}_w$
,
${}^I_a\mathbf{R}$
(or
${}^I_w\mathbf{R}$
) and
$\mathbf{T}_g$
to prevent unneeded matrix inversions and transposes in the measurement equation. We only calibrate either
${}^I_w\mathbf{R}$
or
${}^I_a\mathbf{R}$
since the base "inertial" frame must coincide with one frame arbitrarily. If both
${}^I_w\mathbf{R}$
and
${}^I_a\mathbf{R}$
are calibrated, it will make the rotation between the IMU and camera unobservable due to over parameterization [45] [46] . We define two different models of interested:
KALIBR : Contains
$\mathbf{D}'_{w6}$
,
$\mathbf{D}'_{a6}$
,
${}^I_w\mathbf{R}$
and gravity sensitivity
$\mathbf{T}_{g9}$
. This model follows IMU intrinsic calibration presented in [37] and the output used in the open-source calibration toolbox Kalibr [15] .RPNG : Contains
$\mathbf{D}_{w6}$
,
$\mathbf{D}_{a6}$
, the rotation
${}^I_a\mathbf{R}$
, and gravity sensitivity
$\mathbf{T}_{g9}$
.
$\mathbf{D}_{a6}$
and
$\mathbf{D}_{w6}$
are uptriangular matrices, and follows imu2 analyzed in [45] [46] .It is important to note that one should use the KALIBR model if there is a non-negligible transformation between the gyroscope and accelerometer (it is negligible for most MEMS IMUs, see [29] [38] ) since it assumes the accelerometer frame to be the inertial frame and rigid bodies share the same angular velocity at all points. We can define the following matrices:
\begin{align*} \mathbf{D}_{*6} &= \begin{bmatrix} d_{*1} & d_{*2} & d_{*4} \\ 0 & d_{*3} & d_{*5} \\ 0 & 0 & d_{*6} \end{bmatrix},~~ \mathbf{D}'_{*6} &= \begin{bmatrix} d_{*1} & 0 & 0 \\ d_{*2} & d_{*4} & 0 \\ d_{*3} & d_{*5} & d_{*6} \end{bmatrix},~~ \mathbf{T}_{g9} &= \begin{bmatrix} t_{g1} & t_{g4} & t_{g7} \\ t_{g2} & t_{g5} & t_{g8} \\ t_{g3} & t_{g6} & t_{g9} \end{bmatrix} \end{align*}
In the following derivations, we will compute the Jacobians for all the variables that might appear in the IMU models, including scale/axis correction for gyroscope
$\mathbf{D}_w$
(6 parameters), scale/axis correction for accelerometer
$\mathbf{D}_a$
(6 parameters), rotation from gyroscope to IMU frame
${}^I_w\mathbf{R}$
, rotation from accelerometer to IMU frame
${}^I_a\mathbf{R}$
and gravity sensitivity
$\mathbf{T}_g$
(9 parameters). The state vector contains the IMU state and its intrinsics:
\begin{align*} \mathbf{x}_I & = \begin{bmatrix} \mathbf{x}^{\top}_n & | & \mathbf{x}^{\top}_b & | & \mathbf{x}^{\top}_{in} \end{bmatrix}^{\top} \\ & = \begin{bmatrix} {}^I_G\bar{q}^{\top} & {}^G\mathbf{p}^{\top}_{I} & {}^G\mathbf{v}^{\top}_{I} & | & \mathbf{b}^{\top}_{g} & \mathbf{b}^{\top}_{a} & | & \mathbf{x}^{\top}_{in} \end{bmatrix}^{\top} \\ \mathbf{x}_{in} & = \begin{bmatrix} \mathbf{x}^{\top}_{Dw} & \mathbf{x}^{\top}_{Da} & {}^I_w\bar{q}^{\top} & {}^I_a\bar{q}^{\top} & \mathbf{x}^{\top}_{Tg} \end{bmatrix}^{\top} \end{align*}
where
${}^I_G\bar{q}$
denotes the rotation matrix
${}^I_G\mathbf{R}$
from
$\{G\}$
to
$\{I\}$
.
${}^G\mathbf{p}_{I}$
and
${}^G\mathbf{v}_I$
denote the IMU position and velocity in
$\{G\}$
.
$\mathbf{x}_n$
denotes the IMU navigation states and
$\mathbf{x}_b$
denotes the IMU biases. The IMU intrinsics,
$\mathbf{x}_{in}$
, contain the non-zero elements stored column-wise:
\begin{align*} \mathbf{x}_{D*} & = \begin{bmatrix} d_{*1} & d_{*2} & d_{*3} & d_{*4} & d_{*5} & d_{*6} \end{bmatrix}^{\top} \\ \mathbf{x}_{Tg} & = \begin{bmatrix} t_{g1} & t_{g2} & t_{g3} & t_{g4} & t_{g5} & t_{g6} & t_{g7} & t_{g8} & t_{g9} \end{bmatrix}^{\top} \end{align*}
First we redefine the continuous-time integration equations mentioned in the Continuous-time Inertial Integration section. They are summarized as follows:
\begin{align*} {}^{I_{k+1}}_{G}\mathbf{R} & \triangleq\Delta \mathbf{R}_k ~{}^{I_k}_{G}\mathbf{R} \\ {}^G\mathbf{p}_{I_{k+1}} & \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}} & \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)$
). We have the following integration components:
\begin{align*} \Delta \mathbf{R}_k &= {}^{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} &= \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 \triangleq \boldsymbol{\Xi}_2 \cdot {}^{I_k} \hat{\mathbf{a}} \\ \Delta \mathbf{v}_{k} &= \int^{t_{k+1}}_{t_{k}} {}^{I_k}_{I_\tau}\mathbf{R} {}^{I} \mathbf{a}(t_\tau) ~ d \tau \triangleq \boldsymbol{\Xi}_1 \cdot {}^{I_k} \hat{\mathbf{a}} \end{align*}
where we define the following integrations:
\begin{align*} \boldsymbol{\Xi}_1 & \triangleq \int^{t_{k+1}}_{t_k} \exp \left( {}^{I_\tau}\hat{\boldsymbol{\omega}}\delta\tau \right) d{\tau} \\ \boldsymbol{\Xi}_2 &\triangleq \int^{t_{k+1}}_{t_k} \int^{s}_{t_k} \exp \left( {}^{I_\tau}\hat{\boldsymbol{\omega}}\delta\tau \right) d\tau ds \end{align*}
where
$\delta\tau = t_{\tau} - t_{k}$
and the
$\boldsymbol{\Xi}_1$
and
$\boldsymbol{\Xi}_2$
integration components can be evaluated either analytically (see Integration Component Definitions ) or numerically using RK4.
${}^{I_k}\hat{\boldsymbol{\omega}}$
and
${}^{I_k}\hat{\mathbf{a}}$
are defined as (dropping the timestamp):
\begin{align*} {}^I\hat{\boldsymbol{\omega}} & = {}^I_w\hat{\mathbf{R}} \hat{\mathbf{D}}_{w} {}^w\hat{\boldsymbol{\omega}} \\ {}^w\hat{\boldsymbol{\omega}} & = {}^w\boldsymbol{\omega}_m - \hat{\mathbf{T}}_g {}^I\hat{\mathbf{a}}-\hat{\mathbf{b}}_g = \begin{bmatrix} {}^w\hat{w}_1 & {}^w\hat{w}_2 & {}^w\hat{w}_3 \end{bmatrix}^{\top} \\ {}^I\hat{\mathbf{a}} &= {}^I_a\hat{\mathbf{R}} \hat{\mathbf{D}}_{a} {}^a\hat{\mathbf{a}} \\ {}^a\hat{\mathbf{a}} & = {}^a\mathbf{a}_m - \hat{\mathbf{b}}_a = \begin{bmatrix} {}^a\hat{a}_1 & {}^a\hat{a}_2 & {}^a\hat{a}_3 \end{bmatrix}^{\top} \end{align*}
Under the assumption of constant
${}^{I_k}\hat{\boldsymbol{\omega}}$
and
${}^{I_k}\hat{\mathbf{a}}$
, the above equations are also constant over the time interval. The mean propagation for the new state at
$t_{k+1}$
can be written after taking the expectation:
\begin{align*} {}^{I_{k+1}}_G\hat{\mathbf{R}} & \simeq \Delta \mathbf{R}_k {}^{I_k}_G\hat{\mathbf{R}} \\ {}^G\hat{\mathbf{p}}_{I_{k+1}} & \simeq {}^{G}\hat{\mathbf{p}}_{I_k} + {}^G\hat{\mathbf{v}}_{I_k}\Delta t_k + {}^{I_k}_G\hat{\mathbf{R}}^\top \Delta \hat{\mathbf{p}}_k - \frac{1}{2}{}^G\mathbf{g}\Delta t^2_k \\ {}^G\hat{\mathbf{v}}_{I_{k+1}} & \simeq {}^{G}\hat{\mathbf{v}}_{I_k} + {}^{I_k}_G\hat{\mathbf{R}}^\top \Delta \hat{\mathbf{v}}_k - {}^G\mathbf{g}\Delta t_k \\ \hat{\mathbf{b}}_{g_{k+1}} & = \hat{\mathbf{b}}_{g_k} \\ \hat{\mathbf{b}}_{a_{k+1}} & = \hat{\mathbf{b}}_{a_k} \end{align*}
We first remind the reader of the error states used throughout the system. It is crucial that all error states are consistent between propagation and update. OpenVINS has a type system (see ov_ and Type System ) for which Jacobians need to be derived from. We have the following pose (orientation + position) errors (based on ov_type:: )
\begin{align*} {}^{I}_G \mathbf{R} &\simeq \exp(-\delta \boldsymbol{\theta}) {}^{I}_G \hat{\mathbf{R}} \\ &\simeq (\mathbf{I} - \lfloor \delta \boldsymbol{\theta} \rfloor) {}^{I}_G \hat{\mathbf{R}} \\ {}^{G}\mathbf{p}_I &= {}^{G}\hat{\mathbf{p}}_I + {}^{G}\tilde{\mathbf{p}}_I \end{align*}
For all other vectors (such as IMU intrinsics etc), we leverage simple additive error (based on ov_type:: )
\begin{align*} \mathbf{v} &= \hat{\mathbf{v}} +\tilde{\mathbf{v}} \end{align*}
We first define the
${}^{I_k}\tilde{\boldsymbol{\omega}}$
and
${}^{I_k}\tilde{\mathbf{a}}$
error states. For
${}^I\hat{\mathbf{a}}$
and
${}^I\tilde{\mathbf{a}}$
, we have:
\begin{align*} {}^I{\mathbf{a}} & = {}^I\hat{\mathbf{a}} + {}^I\tilde{\mathbf{a}} \\ &\simeq {}^I_a\hat{\mathbf{R}} \hat{\mathbf{D}}_a \left( {}^a\mathbf{a}_m - \hat{\mathbf{b}}_a \right) + {}^I_a\hat{\mathbf{R}} \mathbf{H}_{Da} \tilde{\mathbf{x}}_{Da} \notag \\ &+ \lfloor {}^I_a\hat{\mathbf{R}} \hat{\mathbf{D}}_a \left( {}^a\mathbf{a}_m - \hat{\mathbf{b}}_a \right) \rfloor \delta \boldsymbol{\theta}_{Ia} - {}^I_a\hat{\mathbf{R}} \hat{\mathbf{D}}_a \tilde{\mathbf{b}}_a - {}^I_a\hat{\mathbf{R}} \hat{\mathbf{D}}_a {\mathbf{n}}_a \\ {}^I\hat{\mathbf{a}} & = {}^I_a\hat{\mathbf{R}} \hat{\mathbf{D}}_a \left( {}^a\mathbf{a}_m - \hat{\mathbf{b}}_a \right) \\ {}^I\tilde{\mathbf{a}} & \simeq {}^I_a\hat{\mathbf{R}} \mathbf{H}_{Da} \tilde{\mathbf{x}}_{Da} + \lfloor {}^I\hat{\mathbf{a}} \rfloor \delta \boldsymbol{\theta}_{Ia} - {}^I_a\hat{\mathbf{R}} \hat{\mathbf{D}}_a \tilde{\mathbf{b}}_a - {}^I_a\hat{\mathbf{R}} \hat{\mathbf{D}}_a {\mathbf{n}}_a \end{align*}
For
${}^I\hat{\boldsymbol{\omega}}$
and
${}^I\tilde{\boldsymbol{\omega}}$
, we have:
\begin{align*} {}^{I}\boldsymbol{\omega} & = {}^{I}\hat{\boldsymbol{\omega}} + {}^{I}\tilde{\boldsymbol{\omega}} \\ & \simeq {}^I_w\hat{\mathbf{R}} \hat{\mathbf{D}}_w \left( {}^w\boldsymbol{\omega}_m - \hat{\mathbf{T}}_g {}^I\hat{\mathbf{a}}-\hat{\mathbf{b}}_g \right) + \lfloor {}^I_w\hat{\mathbf{R}} \hat{\mathbf{D}}_w \left( {}^w\boldsymbol{\omega}_m - \hat{\mathbf{T}}_g {}^I\hat{\mathbf{a}}-\hat{\mathbf{b}}_g \right) \rfloor \delta \boldsymbol{\theta}_{Iw} \\ & + {}^I_w\hat{\mathbf{R}} \mathbf{H}_{Dw} \tilde{\mathbf{x}}_{Dw} - {}^I_w\hat{\mathbf{R}}\hat{\mathbf{D}}_w \left( \hat{\mathbf{T}}_g {}^I\tilde{\mathbf{a}} + \mathbf{H}_{Tg}\tilde{\mathbf{x}}_{Tg} \right) - {}^I_w\hat{\mathbf{R}}\hat{\mathbf{D}}_w \left( \tilde{\mathbf{b}}_g +\mathbf{n}_g \right) \notag \\ {}^{I}\hat{\boldsymbol{\omega}} & = {}^I_w\hat{\mathbf{R}} \hat{\mathbf{D}}_w \left( {}^w\boldsymbol{\omega}_m - \hat{\mathbf{T}}_g {}^I\hat{\mathbf{a}}-\hat{\mathbf{b}}_g \right) \\ {}^{I}\tilde{\boldsymbol{\omega}} & \simeq - {}^I_w\hat{\mathbf{R}}\hat{\mathbf{D}}_w \tilde{\mathbf{b}}_g + {}^I_w\hat{\mathbf{R}}\hat{\mathbf{D}}_w \hat{\mathbf{T}}_g {}^I_a\hat{\mathbf{R}} \hat{\mathbf{D}}_a \tilde{\mathbf{b}}_a + {}^I_w\hat{\mathbf{R}}\mathbf{H}_{Dw}\tilde{\mathbf{x}}_{Dw} \notag \\ & - {}^I_w\hat{\mathbf{R}}\hat{\mathbf{D}}_w \hat{\mathbf{T}}_g {}^I_a\hat{\mathbf{R}}\mathbf{H}_{Da}\tilde{\mathbf{x}}_{Da} + \lfloor {}^I\hat{\boldsymbol{\omega}} \rfloor \delta \boldsymbol{\theta}_{Iw} - {}^I_w\hat{\mathbf{R}}\hat{\mathbf{D}}_w \hat{\mathbf{T}}_g \lfloor {}^I\hat{\mathbf{a}} \rfloor \delta \boldsymbol{\theta}_{Ia} \notag \\ & - {}^I_w\hat{\mathbf{R}}\hat{\mathbf{D}}_w \mathbf{H}_{Tg} \tilde{\mathbf{x}}_{Tg} - {}^I_w\hat{\mathbf{R}} \hat{\mathbf{D}}_w \mathbf{n}_g + {}^I_w\hat{\mathbf{R}}\hat{\mathbf{D}}_w \hat{\mathbf{T}}_g {}^I_a\hat{\mathbf{R}}\hat{\mathbf{D}}_a \mathbf{n}_a \end{align*}
where we need to define
$\mathbf{H}_{Dw}$
,
$\mathbf{H}_{Da}$
and
$\mathbf{H}_{Tg}$
. For the KALIBR model, we have:
\begin{align*} \mathbf{H}_{Dw} & = \begin{bmatrix} {}^w\hat{w}_1 \mathbf{I}_3 & {}^w\hat{w}_2\mathbf{e}_2 & {}^w\hat{w}_2\mathbf{e}_3 & {}^w\hat{w}_3 \mathbf{e}_3 \end{bmatrix} \\ \mathbf{H}_{Da} & = \begin{bmatrix} {}^a\hat{a}_1 \mathbf{I}_3 & & {}^a\hat{a}_2\mathbf{e}_2 & {}^a\hat{a}_2\mathbf{e}_3 & {}^a\hat{a}_3\mathbf{e}_3 \end{bmatrix} \\ \mathbf{H}_{Tg} & = \begin{bmatrix} {}^I\hat{a}_1 \mathbf{I}_3 & {}^I\hat{a}_2 \mathbf{I}_3 & {}^I\hat{a}_3 \mathbf{I}_3 \end{bmatrix} \end{align*}
For the RPNG model, we have:
\begin{align*} \mathbf{H}_{Dw} & = \begin{bmatrix} {}^w\hat{w}_1\mathbf{e}_1 & {}^w\hat{w}_2\mathbf{e}_1 & {}^w\hat{w}_2\mathbf{e}_2 & {}^w\hat{w}_3 \mathbf{I}_3 \end{bmatrix} \\ \mathbf{H}_{Da} & = \begin{bmatrix} {}^a\hat{a}_1\mathbf{e}_1 & {}^a\hat{a}_2\mathbf{e}_1 & {}^a\hat{a}_2\mathbf{e}_2 & {}^a\hat{a}_3 \mathbf{I}_3 \end{bmatrix} \\ \mathbf{H}_{Tg} & = \begin{bmatrix} {}^I\hat{a}_1 \mathbf{I}_3 & {}^I\hat{a}_2 \mathbf{I}_3 & {}^I\hat{a}_3 \mathbf{I}_3 \end{bmatrix} \end{align*}
By summarizing the above equations, we have:
\begin{align*} \begin{bmatrix} {}^{I_k}\tilde{\boldsymbol{\omega}} \\ {}^{I_k}\tilde{\mathbf{a}} \end{bmatrix} & = \begin{bmatrix} \mathbf{H}_b & \mathbf{H}_{in} \end{bmatrix} \begin{bmatrix} \tilde{\mathbf{x}}_{b} \\ \tilde{\mathbf{x}}_{in} \end{bmatrix} + \mathbf{H}_n \begin{bmatrix} \mathbf{n}_{dg} \\ \mathbf{n}_{da} \end{bmatrix} \end{align*}
where we have defined:
\begin{align*} \mathbf{H}_b & = \mathbf{H}_n = \begin{bmatrix} -{}^I_w\hat{\mathbf{R}} \hat{\mathbf{D}}_w & {}^I_w\hat{\mathbf{R}} \hat{\mathbf{D}}_w \hat{\mathbf{T}}_g {}^I_a\hat{\mathbf{R}} \hat{\mathbf{D}}_a \\ \mathbf{0}_3 & -{}^I_a\hat{\mathbf{R}}\hat{\mathbf{D}}_a \end{bmatrix} \\ \mathbf{H}_{in} & = \begin{bmatrix} \mathbf{H}_w & \mathbf{H}_a & \mathbf{H}_{Iw} & \mathbf{H}_{Ia} & \mathbf{H}_{g} \end{bmatrix} \end{align*}
\begin{align*} \mathbf{H}_w & = \frac{\partial [{}^{I_k}\tilde{\boldsymbol{\omega}} ~{}^{I_k}\tilde{\mathbf{a}}]}{\partial \mathbf{x}_{Dw} } = \begin{bmatrix} {}^I_w\hat{\mathbf{R}} \mathbf{H}_{Dw} \\ \mathbf{0}_3 \end{bmatrix} \\ \mathbf{H}_a & = \frac{\partial [{}^{I_k}\tilde{\boldsymbol{\omega}} ~{}^{I_k}\tilde{\mathbf{a}}]}{\partial \mathbf{x}_{Da} } = \begin{bmatrix} - {}^I_w\hat{\mathbf{R}}\hat{\mathbf{D}}_w \hat{\mathbf{T}}_g {}^I_a\hat{\mathbf{R}}\mathbf{H}_{Da} \\ {}^I_a\hat{\mathbf{R}}\mathbf{H}_{Da} \end{bmatrix} \\ \mathbf{H}_{Iw} & = \frac{\partial [{}^{I_k}\tilde{\boldsymbol{\omega}} ~{}^{I_k}\tilde{\mathbf{a}}]}{\partial {}^I_w\delta \boldsymbol{\theta} } = \begin{bmatrix} \lfloor {}^{I}\hat{\boldsymbol{\omega}} \rfloor \\ \mathbf{0}_3 \end{bmatrix} \\ \mathbf{H}_{Ia} & = \frac{\partial [{}^{I_k}\tilde{\boldsymbol{\omega}} ~{}^{I_k}\tilde{\mathbf{a}}]}{\partial {}^I_a\delta \boldsymbol{\theta} } = \begin{bmatrix} -{}^I_a\hat{\mathbf{R}} \hat{\mathbf{D}}_w \hat{\mathbf{T}}_g \lfloor {}^{I}\hat{\mathbf{a}} \rfloor \\ \lfloor {}^I\hat{\mathbf{a}} \rfloor \end{bmatrix} \\ \mathbf{H}_{g} & = \frac{\partial [{}^{I_k}\tilde{\boldsymbol{\omega}} ~{}^{I_k}\tilde{\mathbf{a}}]}{\partial \mathbf{x}_{Tg} } = \begin{bmatrix} -{}^I_w\hat{\mathbf{R}} \hat{\mathbf{D}}_w \mathbf{H}_{Tg} \\ \mathbf{0}_3 \end{bmatrix} \end{align*}
We then linearize
$\Delta \mathbf{R}_k$
,
$\Delta\mathbf{p}_k$
and
$\Delta \mathbf{v}_k$
. For the
$\Delta \mathbf{R}_k$
, we can get:
\begin{align*} \Delta \mathbf{R}_k &= \exp(-{}^{I_{k}}\boldsymbol{\omega}\Delta t_k) \\ &= \exp(-({}^{I_{k}}\hat{\boldsymbol{\omega}} + {}^{I_{k}}\tilde{\boldsymbol{\omega}})\Delta t_k) \\ &\simeq \exp(-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t_k) \cdot \exp(-\mathbf{J}_r (-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t_k) {}^{I_{k}}\tilde{\boldsymbol{\omega}} \Delta t_k) \\ &\simeq \underbrace{\exp(-\Delta \hat{\mathbf{R}}_k\mathbf{J}_r (-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t_k) {}^{I_{k}}\tilde{\boldsymbol{\omega}} \Delta t_k)}_{\Delta \tilde{\mathbf{R}}_k} \exp(-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t_k) \end{align*}
For the integration of
$\Delta \mathbf{p}_k$
, we get:
\begin{align*} \Delta \mathbf{p}_{k} & = \int^{t_{k+1}}_{t_k} \int^{s}_{t_k} \exp({}^{I_{k}}\boldsymbol{\omega}\delta \tau) d \tau d s \cdot {}^{I_{k}} \mathbf{a} \\ & = \int^{t_{k+1}}_{t_k} \int^{s}_{t_k} \exp(({}^{I_{k}} \hat{\boldsymbol{\omega}} + {}^{I_{k}}\tilde{\boldsymbol{\omega}})\delta \tau) d \tau d s \cdot ({}^{I_{k}}\hat{\mathbf{a}} + {}^{I_{k}}\tilde{\mathbf{a}}) \\ & \simeq \int^{t_{k+1}}_{t_k} \int^{s}_{t_k} \exp({}^{I_{k}}\hat{\boldsymbol{\omega}}\delta \tau) \exp(\mathbf{J}_r({}^{I_{k}}\hat{\boldsymbol{\omega}}\delta \tau) {}^{I_{k}}\tilde{\boldsymbol{\omega}}\delta \tau) d \tau d s \cdot ({}^{I_{k}}\hat{\mathbf{a}} + {}^{I_{k}}\tilde{\mathbf{a}}) \\ & \simeq \int^{t_{k+1}}_{t_k} \int^{s}_{t_k} \exp({}^{I_{k}}\hat{\boldsymbol{\omega}}\delta \tau) (\mathbf{I} + \lfloor {\mathbf{J}_r({}^{I_{k}}\hat{\boldsymbol{\omega}}\delta \tau) {}^{I_{k}}\tilde{\boldsymbol{\omega}}\delta \tau} \rfloor) d \tau d s \cdot ({}^{I_{k}}\hat{\mathbf{a}} + {}^{I_{k}}\tilde{\mathbf{a}}) \\ & \simeq \int^{t_{k+1}}_{t_k} \int^{s}_{t_k} \exp({}^{I_{k}}\hat{\boldsymbol{\omega}}\delta \tau) d \tau d s \cdot {}^{I_{k}} \hat{\mathbf{a}} \notag \\ & ~~~ - \underbrace{ \int^{t_{k+1}}_{t_k} \int^{s}_{t_k} \exp({}^{I_{k}}\hat{\boldsymbol{\omega}}\delta \tau) \lfloor {}^{I_{k}} \hat{\mathbf{a}} \rfloor \mathbf{J}_r ({}^{I_{k}}\hat{\boldsymbol{\omega}}\delta \tau) d \tau d s}_{\boldsymbol{\Xi}_4} \cdot {}^{I_{k}}\tilde{\boldsymbol{\omega}} \notag \\ & ~~~ + \underbrace{\int^{t_{k+1}}_{t_k} \int^{s}_{t_k} \exp({}^{I_{k}}\hat{\boldsymbol{\omega}}\delta \tau) d \tau d s }_{\boldsymbol{\Xi}_2} \cdot {}^{I_{k}}\tilde{\mathbf{a}} \notag \\& = \Delta \hat{\mathbf{p}}_{k} \underbrace{- \boldsymbol{\Xi}_4 {}^{I_{k}}\tilde{\boldsymbol{\omega}} +\boldsymbol{\Xi}_2 {}^{I_{k}}\tilde{\mathbf{a}}}_{\Delta \tilde{\mathbf{p}}_k} \\ & = \Delta \hat{\mathbf{p}}_{k} + \Delta \tilde{{\mathbf{p}}}_{k} \end{align*}
For the integration of
$\Delta \mathbf{v}_k$
, we get:
\begin{align*} \Delta \mathbf{v}_{k} & = \int^{t_{k+1}}_{t_k} \exp({}^{I_{k}}\boldsymbol{\omega}\delta \tau) d \tau \cdot {}^{I_{k}} \mathbf{a} \\ & = \int^{t_{k+1}}_{t_k} \exp(({}^{I_{k}} \hat{\boldsymbol{\omega}} + {}^{I_{k}}\tilde{\boldsymbol{\omega}})\delta \tau) d \tau \cdot ({}^{I_{k}}\hat{\mathbf{a}} + {}^{I_{k}}\tilde{\mathbf{a}}) \\ & \simeq \int^{t_{k+1}}_{t_k} \exp({}^{I_{k}}\hat{\boldsymbol{\omega}}\delta \tau) \exp(\mathbf{J}_r({}^{I_{k}}\hat{\boldsymbol{\omega}}\delta \tau) {}^{I_{k}}\tilde{\boldsymbol{\omega}}\delta \tau) d \tau \cdot ({}^{I_{k}}\hat{\mathbf{a}} + {}^{I_{k}}\tilde{\mathbf{a}}) \\ & \simeq \int^{t_{k+1}}_{t_k} \exp({}^{I_{k}}\hat{\boldsymbol{\omega}}\delta \tau) (\mathbf{I} + \lfloor {\mathbf{J}_r({}^{I_{k}}\hat{\boldsymbol{\omega}}\delta \tau) {}^{I_{k}}\tilde{\boldsymbol{\omega}}\delta \tau} \rfloor) d \tau \cdot ({}^{I_{k}}\hat{\mathbf{a}} + {}^{I_{k}}\tilde{\mathbf{a}}) \\ & \simeq \int^{t_{k+1}}_{t_k} \exp({}^{I_{k}}\hat{\boldsymbol{\omega}}\delta \tau) d \tau \cdot {}^{I_{k}} \hat{\mathbf{a}} \notag \\ & ~~~ - \underbrace{ \int^{t_{k+1}}_{t_k} \exp({}^{I_{k}}\hat{\boldsymbol{\omega}}\delta \tau) \lfloor {}^{I_{k}} \hat{\mathbf{a}} \rfloor \mathbf{J}_r ({}^{I_{k}}\hat{\boldsymbol{\omega}}\delta \tau) d \tau }_{\boldsymbol{\Xi}_3} \cdot {}^{I_{k}}\tilde{\boldsymbol{\omega}} \notag \\ & ~~~ + \underbrace{\int^{t_{k+1}}_{t_k} \exp({}^{I_{k}}\hat{\boldsymbol{\omega}}\delta \tau) d \tau }_{\boldsymbol{\Xi}_1} \cdot {}^{I_{k}}\tilde{\mathbf{a}} \notag \\& = \Delta \hat{\mathbf{v}}_{k} \underbrace{- \boldsymbol{\Xi}_3 {}^{I_{k}}\tilde{\boldsymbol{\omega}} +\boldsymbol{\Xi}_1 {}^{I_{k}}\tilde{\mathbf{a}}}_{\Delta \tilde{\mathbf{v}}_k} \\ & = \Delta \hat{\mathbf{v}}_{k} + \Delta \tilde{{\mathbf{v}}}_{k} \end{align*}
where
$\delta \tau = t_\tau - t_k$
and
$\boldsymbol{\Xi}_i, i=1\ldots4$
are shown in the Integration Component Definitions section.
$\mathbf {J}_r(-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t_k)$
is the right Jacobian of
${SO}(3)$
[see ov_core:: [3] ]. In summary, we have the following linearized integration components (see Integration Component Definitions ):
\begin{align*} \Delta \mathbf{R}_k & = \Delta \tilde{\mathbf{R}}_k \Delta \hat{\mathbf{R}}_k \triangleq \exp \left( -\Delta \hat{\mathbf{R}}_k \mathbf{J}_r (\Delta \boldsymbol{\theta}_k) {}^{I_k}\tilde{\boldsymbol{\omega}}\Delta t_k \right) \Delta \hat{\mathbf{R}}_k \\ \Delta \mathbf{p}_k & = \Delta \hat{\mathbf{p}}_k + \Delta \tilde{\mathbf{p}}_k \triangleq \Delta \hat{\mathbf{p}}_k -\boldsymbol{\Xi}_4 {}^{I_k}\tilde{\boldsymbol{\omega}} + \boldsymbol{\Xi}_2 {}^{I_k}\tilde{\mathbf{a}} \\ \Delta \mathbf{v}_k & = \Delta \hat{\mathbf{v}}_k + \Delta \tilde{\mathbf{v}}_k \triangleq \Delta \hat{\mathbf{v}}_k -\boldsymbol{\Xi}_3 {}^{I_k}\tilde{\boldsymbol{\omega}} + \boldsymbol{\Xi}_1 {}^{I_k}\tilde{\mathbf{a}} \end{align*}
where
$\mathbf{J}_r\left(\Delta \boldsymbol{\theta}_k\right) \triangleq \mathbf{J}_r \left(-{}^{I_k}\hat{\boldsymbol{\omega}}_k\Delta t_k \right)$
and
$\boldsymbol{\Xi}_3$
and
$\boldsymbol{\Xi}_4$
are defined as:
\begin{align*} \boldsymbol{\Xi}_3 & = \int^{t_{k+1}}_{t_k} {}^{I_k}_{I_{\tau}}\mathbf{R} \lfloor {}^{I_{\tau}}\mathbf{a} \rfloor \mathbf{J}_r \left( {}^{I_k}\hat{\boldsymbol{\omega}}_k \delta \tau \right) \delta \tau ~d{\tau} \\ \boldsymbol{\Xi}_4 & = \int^{t_{k+1}}_{t_k} \int^{s}_{t_k} {}^{I_k}_{I_{\tau}}\mathbf{R} \lfloor {}^{I_{\tau}}\mathbf{a} \rfloor \mathbf{J}_r \left( {}^{I_k}\hat{\boldsymbol{\omega}}_k \delta \tau \right) \delta \tau ~d{\tau}ds \end{align*}
Hence, the linearized IMU system is:
\begin{align*} \delta \boldsymbol{\theta}_{k+1} & \simeq \Delta \hat{\mathbf{R}}_k \delta \boldsymbol{\theta}_k + \Delta \hat{\mathbf{R}}_k\mathbf{J}_r \left( \Delta \boldsymbol{\theta}_k \right) \Delta t_k {}^{I_k}\tilde{\boldsymbol{\omega}} \\ {}^G\tilde{\mathbf{p}}_{I_{k+1}} & \simeq {}^G\tilde{\mathbf{p}}_{I_{k}}+{}^G\tilde{\mathbf{v}}_k\Delta t_k - {}^{I_{k}}_G\hat{\mathbf{R}}^\top \lfloor \Delta \hat{\mathbf{p}}_k \rfloor \delta \boldsymbol{\theta}_k + {}^{I_{k}}_G\hat{\mathbf{R}}^\top \left( -\boldsymbol{\Xi}_4 {}^{I_k}\tilde{\boldsymbol{\omega}} + \boldsymbol{\Xi}_2 {}^{I_k}\tilde{\mathbf{a}} \right) \\ {}^G\tilde{\mathbf{v}}_{I_{k+1}} & \simeq {}^G\tilde{\mathbf{v}}_{I_{k}} - {}^{I_{k}}_G\hat{\mathbf{R}}^\top \lfloor \Delta \hat{\mathbf{v}}_k \rfloor \delta \boldsymbol{\theta}_k + {}^{I_{k}}_G\hat{\mathbf{R}}^\top \left( -\boldsymbol{\Xi}_3 {}^{I_k}\tilde{\boldsymbol{\omega}} + \boldsymbol{\Xi}_1 {}^{I_k}\tilde{\mathbf{a}} \right) \end{align*}
The overall linearized error state system is:
\begin{align*} \tilde{\mathbf{x}}_{I_{k+1}} & \simeq \boldsymbol{\Phi}_{I{(k+1,k)}}\tilde{\mathbf{x}}_{I_k} + \mathbf{G}_{Ik}\mathbf{n}_{dk} \\ \boldsymbol{\Phi}_{I(k+1,k)} & = \begin{bmatrix} \boldsymbol{\Phi}_{nn} & \boldsymbol{\Phi}_{wa} \mathbf{H}_b & \boldsymbol{\Phi}_{wa} \mathbf{H}_{in} \\ \mathbf{0}_{6\times 9} & \mathbf{I}_6 & \mathbf{0}_{6\times 24} \\ \mathbf{0}_{24\times 9} & \mathbf{0}_{24\times 6} & \mathbf{I}_{24} \end{bmatrix} \\ \mathbf{G}_{Ik} & = \begin{bmatrix} \boldsymbol{\Phi}_{wa} \mathbf{H}_n & \mathbf{0}_{9\times 6} \\ \mathbf{0}_{6} & \mathbf{I}_6 \Delta t_k \\ \mathbf{0}_{24\times 6} & \mathbf{0}_{24\times 6} \end{bmatrix} \end{align*}
where
$\boldsymbol{\Phi}_{I(k+1,k)}$
and
$\mathbf{G}_{Ik}$
are the state transition matrix and noise Jacobians for IMU dynamic model;
$\mathbf{H}_{b}$, $\mathbf{H}_{in}$ and $\mathbf{H}_n$
are Jacobians related to bias, IMU intrinsics and noises, which can be found from previous derivations;
$\mathbf{n}_{dk} = [\mathbf{n}^{\top}_{dg} ~ \mathbf{n}^{\top}_{da} ~ \mathbf{n}^{\top}_{dwg} ~ \mathbf{n}^{\top}_{dwa}]^\top$
is the discrete-time IMU noises;
$\boldsymbol{\Phi}_{nn}$
and
$\boldsymbol{\Phi}_{wa}$
can be computed as:
\begin{align*} \boldsymbol{\Phi}_{nn} & = \begin{bmatrix} \Delta \hat{\mathbf{R}}_k & \mathbf{0}_3 & \mathbf{0}_3 \\ -{}^{I_k}_G\hat{\mathbf{R}}^\top\lfloor \Delta \hat{\mathbf{p}}_k \rfloor & \mathbf{I}_3 & \mathbf{I}_3 \Delta t_k \\ -{}^{I_k}_G\hat{\mathbf{R}}^\top\lfloor \Delta \hat{\mathbf{v}}_k \rfloor & \mathbf{0}_3 & \mathbf{I}_3 \end{bmatrix} \\ \boldsymbol{\Phi}_{wa} & = \begin{bmatrix} \Delta \hat{\mathbf{R}}_k\mathbf{J}_r(\Delta \boldsymbol{\theta}_k) \Delta t_k & \mathbf{0}_3 \\ -{}^{I_k}_G\hat{\mathbf{R}}^\top\boldsymbol{\Xi}_4 & {}^{I_k}_G\hat{\mathbf{R}}^\top\boldsymbol{\Xi}_2 \\ -{}^{I_k}_G\hat{\mathbf{R}}^\top\boldsymbol{\Xi}_3 & {}^{I_k}_G\hat{\mathbf{R}}^\top\boldsymbol{\Xi}_1 \end{bmatrix} \end{align*}
How to apply FEJ to ensure consistency? When using First-Estimate Jacobians (see First-Estimate Jacobian Estimators for an overview) for the state transition matrix, we need to replace
$\Delta \hat{\mathbf{R}}_k$
,
$\Delta \hat{\mathbf{p}}_k$
and
$\Delta \hat{\mathbf{v}}_k$
as:
\begin{align*} \Delta \hat{\mathbf{R}}_k & = {}^{I_{k+1}}_{G}\hat{\mathbf{R}} {}^{I_k}_G\hat{\mathbf{R}}^{\top} \\ \Delta \hat{\mathbf{p}}_k & = {}^{I_k}_G\hat{\mathbf{R}} ( {}^G\hat{\mathbf{p}}_{I_{k+1}} - {}^{G}\hat{\mathbf{p}}_{I_k} - {}^G\hat{\mathbf{v}}_{I_k}\Delta t_k + \frac{1}{2}{}^G\mathbf{g}\Delta t^2_k ) \\ \Delta \hat{\mathbf{v}}_k & = {}^{I_k}_G\hat{\mathbf{R}} ( {}^G\hat{\mathbf{v}}_{I_{k+1}} - {}^{G}\hat{\mathbf{v}}_{I_k} + {}^G\mathbf{g}\Delta t_k ) \end{align*}
This ensure the semi-group property of the state transition matrix by ensuring that the previous timestep is evaluated at the same linearization point (their estimate before update / their estimate when they were first initialized into the state).
Note that
$\mathbf{n}_{d*}\sim \mathcal{N}(\mathbf{0}, \frac{\sigma^2_{*}\mathbf{I}_3}{\Delta t_k})$
and hence the covariance for
$\mathbf{n}_{dI}$
can be written as:
\begin{align*} \mathbf{Q}_{dI} & = \begin{bmatrix} \frac{\sigma^2_g}{\Delta t_k} \mathbf{I}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 \\ \mathbf{0}_3 & \frac{\sigma^2_a}{\Delta t_k} \mathbf{I}_3 & \mathbf{0}_3 & \mathbf{0}_3 \\ \mathbf{0}_3 & \mathbf{0}_3 & \frac{\sigma^2_{wg}}{\Delta t_k} \mathbf{I}_3 & \mathbf{0}_3 \\ \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \frac{\sigma^2_{wa}}{\Delta t_k} \mathbf{I}_3 \end{bmatrix} \end{align*}
Finally, the covariance propagation can be derived as:
\begin{align*} \mathbf{P}_{k+1} & = \boldsymbol{\Phi}_{I(k+1,k)} \mathbf{P}_k \boldsymbol{\Phi}^{\top}_{I(k+1,k)} + \mathbf{G}_{Ik} \mathbf{Q}_{dI} \mathbf{G}^{\top}_{Ik} \end{align*}
When deriving the components, we drop the subscripts for simplicity. As we define angular velocity
$\hat{\omega} = ||{{}^I\hat{\boldsymbol{\omega}}}||$
, rotation axis
$\mathbf{k} = {{{}^I\hat{\boldsymbol{\omega}}}}/{||{{}^I\hat{\boldsymbol{\omega}}}||}$
, and the small interval we are integrating over as
$\delta t$
. The first integration we need is:
\begin{align*} \boldsymbol{\Xi}_1 & = \mathbf{I}_3 \delta t + \frac{1 - \cos (\hat{\omega} \delta t)}{\hat{\omega}} \lfloor \hat{\mathbf{k}} \rfloor + \left(\delta t - \frac{\sin (\hat{\omega} \delta t)}{\hat{\omega}}\right) \lfloor \hat{\mathbf{k}} \rfloor^2 \end{align*}
The second integration we need is:
\begin{align*} \boldsymbol{\Xi}_2 & = \frac{1}{2} \delta t^2 \mathbf{I}_3 + \frac{\hat{\omega} \delta t - \sin (\hat{\omega} \delta t)}{\hat{\omega}^2}\lfloor \hat{\mathbf{k}} \rfloor + \left( \frac{1}{2} \delta t^2 - \frac{1 - \cos (\hat{\omega} \delta t)}{\hat{\omega}^2} \right) \lfloor \hat{\mathbf{k}} \rfloor ^2 \end{align*}
The third integration can be written as:
\begin{align*} \boldsymbol{\Xi}_3 &= \frac{1}{2}\delta t^2 \lfloor \hat{\mathbf{a}} \rfloor + \frac{\sin (\hat{\omega} \delta t_i) - \hat{\omega} \delta t }{\hat{\omega}^2} \lfloor\hat{\mathbf{a}} \rfloor \lfloor \hat{\mathbf{k}} \rfloor \notag \\ & + \frac{\sin (\hat{\omega} \delta t) - \hat{\omega} \delta t \cos (\hat{\omega} \delta t) }{\hat{\omega}^2} \lfloor \hat{\mathbf{k}} \rfloor\lfloor\hat{\mathbf{a}} \rfloor \notag \\ & + \left( \frac{1}{2} \delta t^2 - \frac{1 - \cos (\hat{\omega} \delta t)}{\hat{\omega}^2} \right) \lfloor\hat{\mathbf{a}} \rfloor \lfloor \hat{\mathbf{k}} \rfloor ^2 \notag \\ & + \left( \frac{1}{2} \delta t^2 + \frac{1 - \cos (\hat{\omega} \delta t) - \hat{\omega} \delta t \sin (\hat{\omega} \delta t) }{\hat{\omega}^2} \right) \lfloor \hat{\mathbf{k}} \rfloor ^2 \lfloor\hat{\mathbf{a}} \rfloor \notag \\ & + \left( \frac{1}{2} \delta t^2 + \frac{1 - \cos (\hat{\omega} \delta t) - \hat{\omega} \delta t \sin (\hat{\omega} \delta t) }{\hat{\omega}^2} \right) \hat{\mathbf{k}}^{\top} \hat{\mathbf{a}} \lfloor \hat{\mathbf{k}} \rfloor \notag \\ & - \frac{ 3 \sin (\hat{\omega} \delta t) - 2 \hat{\omega} \delta t - \hat{\omega} \delta t \cos (\hat{\omega} \delta t) }{\hat{\omega}^2} \hat{\mathbf{k}}^{\top} \hat{\mathbf{a}} \lfloor \hat{\mathbf{k}} \rfloor ^2 \end{align*}
The fourth integration we need is:
\begin{align*} \boldsymbol{\Xi}_4 & = \frac{1}{6}\delta t^3 \lfloor\hat{\mathbf{a}} \rfloor + \frac{2(1 - \cos (\hat{\omega} \delta t)) - (\hat{\omega}^2 \delta t^2)}{2 \hat{\omega}^3} \lfloor\hat{\mathbf{a}} \rfloor \lfloor \hat{\mathbf{k}} \rfloor \nonumber \\ & + \left( \frac{2(1- \cos(\hat{\omega} \delta t)) - \hat{\omega} \delta t \sin (\hat{\omega} \delta t)}{\hat{\omega}^3} \right) \lfloor \hat{\mathbf{k}} \rfloor\lfloor\hat{\mathbf{a}} \rfloor \nonumber \\ & + \left( \frac{\sin (\hat{\omega} \delta t) - \hat{\omega} \delta t}{\hat{\omega}^3} + \frac{\delta t^3}{6} \right) \lfloor\hat{\mathbf{a}} \rfloor \lfloor \hat{\mathbf{k}} \rfloor^2 \nonumber \\ & + \frac{\hat{\omega} \delta t - 2 \sin(\hat{\omega} \delta t) + \frac{1}{6}(\hat{\omega} \delta t)^3 + \hat{\omega} \delta t \cos(\hat{\omega} \delta t)}{\hat{\omega}^3} \lfloor \hat{\mathbf{k}} \rfloor^2\lfloor\hat{\mathbf{a}} \rfloor \nonumber \\ & + \frac{\hat{\omega} \delta t - 2 \sin(\hat{\omega} \delta t) + \frac{1}{6}(\hat{\omega} \delta t)^3 + \hat{\omega} \delta t \cos(\hat{\omega} \delta t)}{\hat{\omega}^3} \hat{\mathbf{k}}^{\top} \hat{\mathbf{a}} \lfloor \hat{\mathbf{k}} \rfloor \nonumber \\ & + \frac{4 \cos(\hat{\omega} \delta t) - 4 + (\hat{\omega} \delta t)^2 + \hat{\omega} \delta t \sin(\hat{\omega} \delta t) } {\hat{\omega}^3} \hat{\mathbf{k}}^{\top} \hat{\mathbf{a}} \lfloor \hat{\mathbf{k}} \rfloor^2 \end{align*}
When
$\hat{\omega}$
is too small, in order to avoid numerical instability, we can compute the above integration identities as (see L'Hôpital's rule ):
\begin{align*} \lim\limits_{\hat{\omega} \rightarrow 0} \boldsymbol{\Xi}_1 & = \delta t \mathbf{I}_3 + \delta t \sin (\hat{\omega} \delta t_i) \lfloor \hat{\mathbf{k}} \rfloor + \delta t \left( 1 - \cos(\hat{\omega} \delta t) \right) \lfloor \hat{\mathbf{k}} \rfloor^2 \\ \lim\limits_{\hat{\omega} \rightarrow 0} \boldsymbol{\Xi}_2 & = \frac{\delta t^2}{2} \mathbf{I}_3 + \frac{\delta t^2}{2} \sin(\hat{\omega} \delta t) \lfloor \hat{\mathbf{k}} \rfloor + \frac{\delta t^2}{2} \left( 1 - \cos(\hat{\omega} \delta t) \right) \lfloor \hat{\mathbf{k}} \rfloor ^2 \\ & = \frac{\delta t}{2} \lim\limits_{\hat{\omega} \rightarrow 0} \boldsymbol{\Xi}_1 \\ \lim\limits_{\hat{\omega} \rightarrow 0} \boldsymbol{\Xi}_3 & = \frac{\delta t^2}{2} \lfloor \hat{\mathbf{a}} \rfloor + \frac{\delta t^2 \sin (\hat{\omega} \delta t)}{2} \left( - \lfloor \hat{\mathbf{a}} \rfloor \lfloor \hat{\mathbf{k}} \rfloor + \lfloor \hat{\mathbf{k}} \rfloor \lfloor \hat{\mathbf{a}} \rfloor + \hat{\mathbf{k}}^{\top} \hat{\mathbf{a}} \lfloor \hat{\mathbf{k}} \rfloor^2 \right) \notag \\ & + \frac{\delta t^2}{2} (1 - \cos (\hat{\omega} \delta t)) \left( \lfloor \hat{\mathbf{a}} \rfloor \lfloor \hat{\mathbf{k}} \rfloor^2 + \lfloor \hat{\mathbf{k}} \rfloor^2 \lfloor \hat{\mathbf{a}} \rfloor + \hat{\mathbf{k}}^{\top} \hat{\mathbf{a}} \lfloor \hat{\mathbf{k}} \rfloor \right) \\ \lim\limits_{\hat{\omega} \rightarrow 0} \boldsymbol{\Xi}_4 &= \frac{\delta t^3}{6} \lfloor \hat{\mathbf{a}} \rfloor + \frac{\delta t^3 \sin (\hat{\omega} \delta t)}{6} \left( - \lfloor \hat{\mathbf{a}} \rfloor \lfloor \hat{\mathbf{k}} \rfloor + \lfloor \hat{\mathbf{k}} \rfloor \lfloor \hat{\mathbf{a}} \rfloor + \hat{\mathbf{k}}^{\top} \hat{\mathbf{a}} \lfloor \hat{\mathbf{k}} \rfloor^2 \right) \notag \\ & + \frac{\delta t^3}{6} (1 - \cos (\hat{\omega} \delta t)) \left( \lfloor \hat{\mathbf{a}} \rfloor \lfloor \hat{\mathbf{k}} \rfloor^2 + \lfloor \hat{\mathbf{k}} \rfloor^2 \lfloor \hat{\mathbf{a}} \rfloor + \hat{\mathbf{k}}^{\top} \hat{\mathbf{a}} \lfloor \hat{\mathbf{k}} \rfloor \right) \\ & = \frac{\delta t}{3} \lim\limits_{\hat{\omega} \rightarrow 0} \boldsymbol{\Xi}_3 \end{align*}
We currently do not implement a RK4 version of the above integrations. This would likely improve performance if leveraged as it would allow for the measurement to evolve over the integration period.