IMU Propagation: State Integration in OpenVINS EKF

OpenVINS uses a 6-axis IMU as the backbone of its Extended Kalman Filter (EKF), propagating a 15-dimensional inertial state forward between camera frames. Each incoming IMU reading — a raw angular rate and a raw linear acceleration — is de-biased and integrated to evolve the orientation, position, velocity, and sensor biases in time. Two integration strategies are supported: a straightforward discrete model that is easy to understand and implement, and a full continuous-time analytical model that additionally accommodates IMU intrinsic calibration parameters such as scale factors, axis misalignments, and gyroscope gravity sensitivity.

IMU Measurement Model

A 6-axis MEMS IMU reports measurements of the local angular rate

{}^I\boldsymbol{\omega}_m

and local linear acceleration

{}^I\mathbf{a}_m

in the IMU body frame

\{I\}

. Both channels are corrupted by an additive slowly-drifting bias and white Gaussian noise:

{}^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)

where

{}^I\boldsymbol{\omega}

and

{}^I\mathbf{a}

are the true angular velocity and linear acceleration,

\mathbf{b}_g

and

\mathbf{b}_a

are the gyroscope and accelerometer biases,

\mathbf{n}_g

and

\mathbf{n}_a

are zero-mean white Gaussian noise processes,

{}^G\mathbf{g} = [0\ 0\ 9.81]^\top

is gravity expressed in the global frame

\{G\}

, and

{}^I_G\mathbf{R}

rotates vectors from the global frame into the IMU frame. For IMUs with significant scale/axis imperfections (MEMS models in particular), OpenVINS also supports an extended intrinsic model that introduces separate gyroscope and accelerometer sensor frames

\{w\}

and

\{a\}

{}^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}\bigl({}^I\mathbf{a}(t) + {}^I_G\mathbf{R}(t)\,{}^G\mathbf{g}\bigr) + \mathbf{b}_a(t) + \mathbf{n}_a(t)

Here

\mathbf{T}_w

and

\mathbf{T}_a

are invertible

3\times3

scale/misalignment matrices, and

\mathbf{T}_g

models the gravity sensitivity of the gyroscope (its response to linear acceleration due to inherent inertia).

Calibrating IMU intrinsics introduces many additional degrees of freedom and requires trajectories with sufficient excitation (e.g. dynamic handheld motion). Planar, single-axis, or constant-measurement trajectories can lead to degenerate calibrations. OpenVINS recommends performing intrinsic calibration offline unless your platform is guaranteed to have rich, multi-axis motion.

Inertial State Vector

The inertial navigation state

\mathbf{x}_I

at time

t

is a 16-dimensional vector (parameterized as a 15-dimensional error state):

\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}

Symbol	Dimension	Meaning
${}^I_G\bar{q}$	4 (unit quaternion)	Rotation from global frame $\{G\}$ to IMU frame $\{I\}$ (JPL convention)
${}^G\mathbf{p}_I$	3	Position of IMU origin in the global frame
${}^G\mathbf{v}_I$	3	Velocity of IMU in the global frame
$\mathbf{b}_g$	3	Gyroscope bias
$\mathbf{b}_a$	3	Accelerometer bias

OpenVINS uses the JPL quaternion convention with a left multiplicative quaternion error:

{}^I_G\bar{q} = \delta\bar{q} \otimes {}^I_G\hat{\bar{q}}, \qquad \delta\bar{q} \simeq \begin{bmatrix}\frac{1}{2}\tilde{\boldsymbol{\theta}} \\ 1\end{bmatrix}

where

\tilde{\boldsymbol{\theta}} \in \mathbb{R}^3

is the small-angle orientation error vector. All other states use standard additive error. This yields a 15-dimensional error state

\tilde{\mathbf{x}}_I

despite the 16-element quaternion parameterization.

IMU Kinematic Equations

The continuous-time differential equations governing IMU state evolution are:

{}^I_G\dot{\bar{q}}(t) = \frac{1}{2}\boldsymbol{\Omega}({}^I\boldsymbol{\omega}(t))\,{}^{I_t}_G\bar{q}

{}^G\dot{\mathbf{p}}_I(t) = {}^G\mathbf{v}_I(t)

{}^G\dot{\mathbf{v}}_I(t) = {}^{I_t}_G\mathbf{R}^\top\,{}^I\mathbf{a}(t)

\dot{\mathbf{b}}_g(t) = \mathbf{n}_{wg}(t), \qquad \dot{\mathbf{b}}_a(t) = \mathbf{n}_{wa}(t)

where

\boldsymbol{\Omega}(\boldsymbol{\omega}) = \begin{bmatrix} -\lfloor\boldsymbol{\omega}\times\rfloor & \boldsymbol{\omega} \\ -\boldsymbol{\omega}^\top & 0\end{bmatrix}

is the quaternion rate matrix, and the biases evolve as random walks driven by white Gaussian process noises

\mathbf{n}_{wg}

and

\mathbf{n}_{wa}

Integration Strategies

Given the kinematic equations above, the state mean must be numerically integrated between IMU timestamps. OpenVINS provides two integration approaches. Both compute the state transition matrix

\boldsymbol{\Phi}(t_{k+1},t_k)

and noise Jacobian

\mathbf{G}_k

, then propagate the covariance as:

\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

Discrete Integration
Analytical Integration

The discrete model assumes each IMU measurement is constant over its sampling interval

\Delta t_k = t_{k+1} - t_k

. This is a zero-order hold (ZOH) approximation.Quaternion propagation uses the matrix-exponential form of the constant-rate assumption:

{}^{I_{k+1}}_G\hat{\bar{q}} = \exp\!\Bigl(\tfrac{1}{2}\boldsymbol{\Omega}(\hat{\boldsymbol{\omega}}_k)\Delta t_k\Bigr)\,{}^{I_k}_G\hat{\bar{q}}

where

\hat{\boldsymbol{\omega}}_k = \boldsymbol{\omega}_{m,k} - \hat{\mathbf{b}}_{g,k}

is the bias-corrected angular rate.Position and velocity are updated assuming constant acceleration in the current body frame:

{}^G\hat{\mathbf{p}}_{I_{k+1}} = {}^G\hat{\mathbf{p}}_{I_k} + {}^G\hat{\mathbf{v}}_{I_k}\Delta t_k - \tfrac{1}{2}{}^G\mathbf{g}\Delta t_k^2 + \tfrac{1}{2}{}^{I_k}_G\hat{\mathbf{R}}^\top\hat{\mathbf{a}}_k\Delta t_k^2

{}^G\hat{\mathbf{v}}_{k+1} = {}^G\hat{\mathbf{v}}_{I_k} - {}^G\mathbf{g}\Delta t_k + {}^{I_k}_G\hat{\mathbf{R}}^\top\hat{\mathbf{a}}_k\Delta t_k

Biases propagate as pure random walks:

\hat{\mathbf{b}}_{g,k+1} = \hat{\mathbf{b}}_{g,k}, \qquad \hat{\mathbf{b}}_{a,k+1} = \hat{\mathbf{b}}_{a,k}

The discrete noise covariance is converted from the continuous-time noise spectral density:

\mathbf{Q}_{meas} = \begin{bmatrix} \frac{\sigma_{g_c}^2}{\Delta t}\mathbf{I}_3 & \mathbf{0} \\ \mathbf{0} & \frac{\sigma_{a_c}^2}{\Delta t}\mathbf{I}_3\end{bmatrix}, \quad \mathbf{Q}_{bias} = \begin{bmatrix} \Delta t\,\sigma_{wg_c}^2\mathbf{I}_3 & \mathbf{0} \\ \mathbf{0} & \Delta t\,\sigma_{wa_c}^2\mathbf{I}_3\end{bmatrix}

The 15×15 state transition matrix

\boldsymbol{\Phi}

is constructed by perturbing each state variable and collecting first-order terms. It captures how orientation error propagates into position and velocity errors through the estimated acceleration.

The discrete model is simpler to implement and is the right starting point for understanding OpenVINS propagation. It does not include IMU intrinsic calibration parameters.

The analytical model performs continuous-time integration under the constant-measurement assumption and accommodates the full IMU intrinsic model (scale/axis corrections

\mathbf{D}_w

\mathbf{D}_a

; inter-frame rotations

{}^I_w\mathbf{R}

{}^I_a\mathbf{R}

; and gravity sensitivity

\mathbf{T}_g

).The continuous integration introduces four matrix integrals

\boldsymbol{\Xi}_{1\ldots4}

that are evaluated analytically using closed-form expressions involving

SO(3)

exponentials:

\boldsymbol{\Xi}_1 \triangleq \int_{t_k}^{t_{k+1}} {}^{I_k}_{I_\tau}\mathbf{R}\,d\tau, \qquad \boldsymbol{\Xi}_2 \triangleq \int_{t_k}^{t_{k+1}}\int_{t_k}^{s} {}^{I_k}_{I_\tau}\mathbf{R}\,d\tau\,ds

These allow the position and velocity increments to be written as:

\Delta\hat{\mathbf{p}}_k = \boldsymbol{\Xi}_2\cdot{}^{I_k}\hat{\mathbf{a}}, \qquad \Delta\hat{\mathbf{v}}_k = \boldsymbol{\Xi}_1\cdot{}^{I_k}\hat{\mathbf{a}}

Intrinsic calibration adds up to 27 extra parameters (6 gyroscope, 6 accelerometer, 3 inter-frame rotation, 9 gravity sensitivity) to the state, and the analytical Jacobians with respect to all of them are derived in closed form. Two IMU intrinsic models are supported:

KALIBR: Lower-triangular $\mathbf{D}'_{w6}$ / $\mathbf{D}'_{a6}$ , calibrates ${}^I_w\mathbf{R}$ ; compatible with the Kalibr toolbox output.
RPNG: Upper-triangular $\mathbf{D}_{w6}$ / $\mathbf{D}_{a6}$ , calibrates ${}^I_a\mathbf{R}$ ; follows the imu2 model analyzed in Yang et al. (TRO 2023).

Only one of

{}^I_w\mathbf{R}

{}^I_a\mathbf{R}

should be calibrated at a time. Calibrating both over-parameterizes the rotation between the IMU and camera, making that extrinsic unobservable.

First-Estimate Jacobians (FEJ) for Consistency

A standard EKF linearizes about the current (post-update) state estimate at each step. Because a visual-inertial system has 4 unobservable degrees of freedom — absolute yaw and 3D global translation — linearizing about an ever-changing estimate causes the observability matrix to gain spurious rank along the yaw direction, leading to filter over-confidence (estimated uncertainty smaller than true uncertainty) and degraded accuracy. First-Estimate Jacobians (FEJ) resolve this by evaluating all Jacobians at a fixed, per-state linearization point:

Propagation: The state transition matrix $\boldsymbol{\Phi}(t_{k+1},t_k)$ is evaluated using the pre-update (predicted) state rather than the post-update state. This preserves the semi-group property $\boldsymbol{\Phi}(t_{k+1},t_{k-1}) = \boldsymbol{\Phi}(t_{k+1},t_k)\boldsymbol{\Phi}(t_k,t_{k-1})$ .
Update: The measurement Jacobian $\mathbf{H}_k$ for a feature is computed at the feature’s initial triangulated estimate (not its current estimate) for all subsequent updates.

In the analytical model, applying FEJ means replacing the integrated increments with expressions computed from the current and previous state estimates:

\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}}\Bigl({}^G\hat{\mathbf{p}}_{I_{k+1}} - {}^G\hat{\mathbf{p}}_{I_k} - {}^G\hat{\mathbf{v}}_{I_k}\Delta t_k + \tfrac{1}{2}{}^G\mathbf{g}\Delta t_k^2\Bigr)

This guarantees the correct 4-dimensional unobservable nullspace and prevents the filter from falsely gaining information along the global yaw direction.

The `Propagator` Class

The Propagator class (ov_msckf/src/state/Propagator.h) implements the IMU propagation pipeline. Inputs:

A stream of ImuData messages (timestamp, angular rate $\boldsymbol{\omega}_m$ , linear acceleration $\mathbf{a}_m$ ) fed via feed_imu().
A NoiseManager struct holding the continuous-time noise spectral densities ( $\sigma_g$ , $\sigma_a$ , $\sigma_{wg}$ , $\sigma_{ab}$ ).
A gravity magnitude scalar (normally 9.81 m/s²).

Key methods:

Method	Description
`feed_imu(message, oldest_time)`	Appends a new IMU reading to the internal buffer and trims measurements older than `oldest_time`.
`propagate_and_clone(state, timestamp)`	Collects all IMU readings since the last state time, integrates the mean and covariance step-by-step, then clones the current IMU pose into the sliding window.
`fast_state_propagate(state, timestamp, state_plus, cov)`	Propagates a copy of the IMU state to an arbitrary future timestamp without modifying the filter state — used to provide high-frequency pose outputs between camera updates.
`select_imu_readings(...)`	Static helper that extracts and (linearly) interpolates IMU readings in a given time window.

Propagator maintains a cache that is invalidated after each EKF update. Subsequent calls to fast_state_propagate re-use a cached propagated state when possible to minimize redundant computation.

Get Started

Guides

Theory

Evaluation

Development

IMU Propagation: State Integration in OpenVINS EKF

IMU Measurement Model

Inertial State Vector

IMU Kinematic Equations

Integration Strategies

First-Estimate Jacobians (FEJ) for Consistency

The `Propagator` Class

Build docs developers (and LLMs) love

Get Started

Guides

Theory

Evaluation

Development

Documentation Index

​IMU Measurement Model

​Inertial State Vector

​IMU Kinematic Equations

​Integration Strategies

​First-Estimate Jacobians (FEJ) for Consistency

​The Propagator Class

Build docs developers (and LLMs) love

IMU Measurement Model

Inertial State Vector

IMU Kinematic Equations

Integration Strategies

First-Estimate Jacobians (FEJ) for Consistency

The `Propagator` Class