IMU Prediction

Error state Kalman filter in brief¶

In error-state filter formulations, we speak of true-, nominal- and error-state values, the true-state being expressed as a suitable composition (linear sum, quaternion product or matrix product) of the nominal- and the error- states. The idea is to consider the nominal-state as large-signal (integrable in non-linear fashion) and the error-state as small signal (thus linearly integrable and suitable for linear-Gaussian filtering).

The error-state filter can be explained as follows. On one side, high-frequency IMU data $u_{m}$ is integrated into a nominal-state $x$. This nominal state does not take into account the noise terms $w$ and other possible model imperfections. As a consequence, it will accumulate errors. These errors are collected in the error-state $δx$ and estimated with the Error-State Kalman Filter (ESKF), this time incorporating all the noise and perturbations. The error-state consists of small-signal magnitudes, and its evolution function is correctly defined by a (time-variant) linear dynamic system, with its dynamic, control and measurement matrices computed from the values of the nominal-state. In parallel with integration of the nominal-state, the ESKF predicts a Gaussian estimate of the error-state. It only predicts, because by now no other measurement is available to correct these estimates. The filter correction is performed at the arrival of information other than IMU (e.g. GPS, vision, etc.), which is able to render the errors observable and which happens generally at a much lower rate than the integration phase. This correction provides a posterior Gaussian estimate of the error-state. After this, the error-state’s mean is injected into the nominal-state, then reset to zero. The error-state’s covariances matrix is conveniently updated to reflect this reset. The system goes on like this forever.

The simplified ESKF goes like this:

1. Use IMU to propagate mean simply by ignoring noise

2. Error state equations give you state transition and noise jacobians. These are only needed for covariance propagation. Mean of error state is zero.

3. Update step updates the covariance using measurement jacobians and the mean error state which is injected into the mean as the next estimate.

Solas Prediction¶

Note: Sola uses Hamilton convention. Please check the assumptions in Solas paper.

For continuos system of equations:

True state Kinematics¶

The true acceleration $\mathbf{a}_t$ and angular rate $\omega_t$ are obtained from an IMU in the form of noisy sensor readings $\mathbf{a}_m$ and $\omega_m$ in body frame, namely:

with $\mathbf{R}_t \triangleq \mathbf{R}\{\mathbf{q}_t\}$. With this, the true values can be isolated (this means that we have inverted the measurement equations),

Substituting above yields the kinematic system

which we may name $\dot{\mathbf{x}}_t = f_t(\mathbf{x}_t, \mathbf{u}, \mathbf{w})$. This system has state $\mathbf{x}_t$, is governed by IMU noisy readings $\mathbf{u}_m$, and is perturbed by white Gaussian noise $\mathbf{w}$, all defined by

Nominal state Kinematics¶

The nominal-state kinematics corresponds to the modeled system without noises or perturbations,

Error-state kinematics¶

The goal is to determine the linearized dynamics of the error-state. For each state equation, we write its composition, linearizing wherever needed, solving for the error state and simplifying all second-order infinitesimals.

Now we move to discrete case:

True state Kinematics¶

The above true state kinematics for the continuous case can be converted to true state by converting those equations into difference equations. It should be straightforward.

Nominal state Kinematics¶

We can write the differences equations of the nominal-state as

where $x \leftarrow f(x, \bullet)$ stands for a time update of the type $x_{k+1} = f(x_k, \bullet_k)$, $\mathbf{R} \triangleq \mathbf{R}\{\mathbf{q}\}$ is the rotation matrix associated to the current nominal orientation $\mathbf{q}$, and $\mathbf{q}\{v\}$ is the quaternion associated to the rotation $v$.

We can also use more precise integration. Check RK4 methods.

Error-state kinematics¶

The deterministic part is integrated normally, and the integration of the stochastic part results in random impulses, thus,

Here, $\mathbf{v}_{\mathbf{i}}$, $\theta_{\mathbf{i}}$, $\mathbf{a}_{\mathbf{i}}$ and $\omega_{\mathbf{i}}$ are the random impulses applied to the velocity, orientation and bias estimates, modeled by white Gaussian processes. Their mean is zero, and their covariances matrices are obtained by integrating the covariances of $\mathbf{a}_n, \omega_n, \mathbf{a}_w$ and $\omega_w$ over the step time $\Delta t$,

where $\sigma_{\mathbf{a}_n} [m/s^2]$, $\sigma_{\omega_n} [rad/s]$, $\sigma_{\mathbf{a}_w} [m/s^2\sqrt{s}]$ and $\sigma_{\omega_w} [rad/s\sqrt{s}]$ are to be determined from the information in the IMU datasheet, or from experimental measurements.

Error-state Jacobian and perturbation matrices¶

The Jacobians are obtained by simple inspection of the error-state differences equations in the previous section.

To write these equations in compact form, we consider the nominal state vector $\mathbf{x}$, the error state vector $\delta \mathbf{x}$, the input vector $\mathbf{u}_m$, and the perturbation impulses vector $\mathbf{i}$, as follows,

The error-state system is now

The ESKF prediction equations are written:

where $\delta \mathbf{x} \sim \mathcal{N}\{\hat{\delta \mathbf{x}}, \mathbf{P}\}^{25}$; $\mathbf{F}_{\mathbf{x}}$ and $\mathbf{F}_{\mathbf{i}}$ are the Jacobians of $f()$ with respect to the error and perturbation vectors; and $\mathbf{Q}_{\mathbf{i}}$ is the covariances matrix of the perturbation impulses.

The expressions of the Jacobian and covariances matrices above are detailed below. All state-related values appearing herein are extracted directly from the nominal state.

Openvins Prediction¶

Note: Openvins uses JPL convention. Please check the papers for assumptions. Referenes:

As before we start with the continuos case.

True state kinematics¶

Openvins state is of the form:

The IMU state evolves over time as follows:

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\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{\omega}(t)$ and $^I\hat{\mathbf{a}}(t)$, we can define the solutions to the above IMU kinematics differential equation.

where $\Delta t_k = t_{k+1} - t_k$, ${^{I_k}_{I_\tau}\mathbf{R}} = \exp(\int_{t_k}^{t_\tau} {}^I\omega(t_u) \, du)$, $\exp(\cdot)$ is the $SO(3)$ matrix exponential, 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.

Openvins Discrete Propagation¶

True state¶

Assumptions:

1. Simple IMU model

2. Hold initial measuement constant over the period including R which in pratice changes since angular velocity is constant over the interval

3. We 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 $\omega_m(t_k) = \omega_{m,k}$ and constant $\mathbf{a}_m(t_k) = \mathbf{a}_{m,k}$ over $t \in [t_k, t_{k+1}]$. 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:

where $\Delta t_k = t_{k+1} - t_k$.

Note that the noises used here are the discrete ones which need to be converted from their continuous-time versions. In particular, when the covariance matrix of the continuous-time measurement noises is given by $\mathbf{Q}_c$ and $\mathbf{n}_c = [\mathbf{n}_{g_c} \mathbf{n}_{a_c} \mathbf{n}_{wg_c} \mathbf{n}_{wa_c}]^\top$, then the discrete-time noise covariance $\mathbf{Q}_d$ can be computed as (see [Indirect Kalman Filter for 3D Attitude Estimation] Eq. (129) and (130)):

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

Nominal and Error state kinematics¶

For the error propagation 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:

where $\tilde{\omega} = \omega - \hat{\omega} = -(\tilde{\mathbf{b}}_g + \mathbf{n}_g)$ handles both the perturbation to the bias and measurement noise. $\mathbf{J}_r(\theta)$ is the right Jacobian of $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. Above we have defined ${^{I_{k+1}}_{I_k}\hat{\theta}} = -{^{I_k}\hat{\omega}} \Delta t_k$. $\tilde{\mathbf{a}} = \mathbf{a} - \hat{\mathbf{a}} = -(\tilde{\mathbf{b}}_a + \mathbf{n}_a)$.

In order to propagate the covariance matrix, we should derive the linearized error-state propagation, i.e., computing the system Jacobian $\Phi(t_{k+1}, t_k)$ and noise Jacobian $\mathbf{G}_k$.

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

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

Openvins Analytical Propagation¶

State and IMU Intrinsics Model¶

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.

where $\mathbf{D}_w = \mathbf{T}_w^{-1}$ and $\mathbf{D}_a = \mathbf{T}_a^{-1}$. Note that here we have factored out the $-{}^I_G\mathbf{R}(t){}^G\mathbf{g}$ gravity term. 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.

KALIBR Contains $\mathbf{D}_{w6}$, $\mathbf{D}_{a6}$, ${}^I_w\mathbf{R}$ and gravity sensitivity $\mathbf{T}_{g9}$. 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) 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:

The state vector contains the IMU state and its intrinsics:

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:

True state Kinematics¶

First we redefine the continuous-time integration equations mentioned in the Continuous-time Inertial Integration section. They are summarized as follows:

where $\Delta t_k = t_{k+1} - t_k$, ${}^I_{I_\tau}\mathbf{R} = \exp(\int_{t_k}^{t_\tau} {}^I\boldsymbol{\omega}(t_u) \, du)$, $\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: