Tutorial on Aided Inertial Navigation Systems: A Modern Treatment Using Lie-Group Theoretical Methods

Soulaimane Berkane

cs.ROMar 7, 2026

Tutorial on Aided Inertial Navigation Systems: A Modern Treatment Using Lie-Group Theoretical Methods

Authors: Soulaimane Berkane

Organizations: Université du Québec en Outaouais · LaRSA – Robotics and Autonomous Systems Laboratory

Abstract

Robots and autonomous vehicles rely on accurate estimates of their position, velocity, and orientation. Inertial sensors measure motion at high rates, but errors accumulate as these measurements are integrated. Satellite navigation, cameras, and other sensors help limit this drift. Combining these sources effectively requires estimation methods that account for the geometry of the navigation state. This tutorial presents a unified introduction to aided inertial navigation from a Lie-group perspective. It connects motion prediction, uncertainty propagation, and measurement updates within a common geometric framework. Particular attention is given to invariant filtering, which uses system symmetries to simplify the description and evolution of estimation errors. The tutorial explains how choices of state and error representations shape the filter equations. It connects theoretical principles with formulas suitable for implementation and discusses recent extensions addressing convergence and sensor biases. By organizing established and emerging methods around their shared geometric foundations, the presentation provides students, researchers, and engineers with a basis for understanding, implementing, and extending navigation algorithms.

Figures & tables

Figure 1: INS kinematics setting. The North–East–Down (NED) inertial frame

\{\mathcal{I}\}

is defined by the basis

(e_{1},e_{2},e_{3})

. The body-fixed IMU frame

\{\mathcal{B}\}

has basis vectors

(e_{1b},e_{2b},e_{3b})

. The vehicle position expressed in

\{\mathcal{I}\}

p

, and its orientation is represented by the rotation matrix

R\in\mathbb{SO}(3)

, which maps body-frame vectors into the inertial frame.

Panel of Figure 2 — Figure 2: MEMS gyroscope structure used in inertial sensing . (a) A two-dimensional schematic of a vibrating MEMS gyroscope in which a driven proof mass experiences a Coriolis-induced motion proportional to the angular rate. (b) A tri-axis MEMS gyroscope with three orthogonal vibrating structures that measure the body-frame angular-velocity components about the $x$ , $y$ , and $z$ axes.

Panel of Figure 3 — Figure 3: MEMS accelerometer structure used in inertial sensing . (a) A two-dimensional view of a capacitive one-axis MEMS accelerometer in which a suspended proof mass deflects under acceleration, producing a differential change in capacitance. (b) A tri-axis accelerometer consisting of three orthogonal sensing axes that measure the specific-force components along the $x$ , $y$ , and $z$ body-frame directions.

Panel of Figure 4 — Figure 4: Schematic overview of the recursive bias-free INS estimation architecture. The state is propagated forward in time using high-rate IMU measurements through the Lie-group process model ( 10 ) (prediction step), whose exact discrete-time update is given in ( 20 ), together with the associated covariance propagation in ( 41 ). The state is then intermittently corrected using measurements from aiding sensors (update step) via the computation of the innovation ( 58 ). The Kalman gain $K_{k}$ is computed according to ( 59 )–( 60 ).

Figure 5: Exact versus classical IMU discretization for circular motion . Comparison of discrete-time propagation under piecewise-constant IMU inputs. The left panel shows the exact Lie-group propagation on

\mathbb{SE}_{2}(3)

for multiple sampling intervals

\Delta t

, where the circular trajectory is preserved independently of

\Delta t

. The right panel shows the classical INS discretization (

J_{\ell}\approx I

Q_{\ell}\approx\tfrac{1}{2}I

), for which discretization errors accumulate as

\Delta t

increases, resulting in visible geometric distortion of the trajectory.

Figure 6: Exact log-linear versus conventional linearized error propagation. Position mismatch between the exact nonlinear error and the corresponding error-propagation model for a

20^{\circ}

initial attitude error. The invariant log-linear prediction agrees with the nonlinear error up to numerical precision, whereas the trajectory-dependent first-order model ( 31 ) exhibits an increasing mismatch. Solid and dashed lines denote straight and circular motion, respectively. No measurement updates are applied; the comparison concerns deterministic error propagation only.

Figure 7: Extended-pose propagation on the Lie group

\mathbb{SE}_{2}(3)

. The mean estimate

\hat{X}_{k}

evolves according to ( 20 ), while the associated uncertainty follows ( 41 ). Ellipsoids represent the error covariance, illustrating how uncertainty accumulates over successive IMU propagation steps.

Figure 8: Comparison of direct-product and right-invariant error distributions . Each panel shows multiple distributions with identical covariance and means located at different positions along the

x

-axis. For each mean pose, approximately

230

Monte Carlo samples are drawn in the Lie algebra from a zero-mean Gaussian distribution with standard deviations

(10^{\circ},\,0.8\,\mathrm{m},\,1.6\,\mathrm{m})

on the yaw–

x

–

y

components and mapped to the group. The position components are shown in the

x

–

y

plane, while the yaw component is encoded by the orientation of each line segment. Left: direct-product error model on

\mathbb{SO}(3)\times\mathbb{R}^{6}

[ forster2016manifold ] , which produces ellipsoidal position uncertainty independent of the distance from the origin. Right: right-invariant error model on

\mathbb{SE}_{2}(3)

, where the nonlinear group mapping yields curved (banana-shaped) distributions whose spread increases with the distance from the origin due to the presence of the

\hat{p}_{k}\times\delta\phi_{k}

term in ( 37 ).

Figure 9: Simple propagation on $\mathbb{SE}_{2}(3)$ with $(\hat{X}_{0},\Sigma_{0})=(I_{5},0_{9\times 9})$ , $(\omega,a)=(0_{3},e_{1}-g)$ , sampling period $\Delta t=0.05\,\mathrm{s}$ , and propagation duration $15\,\mathrm{s}$ . The gyroscope is corrupted by discrete-time noise along the $z$ -axis with standard deviation $\sigma_{\omega_{z}}=20^{\circ}/\mathrm{s}$ per sample, while accelerometer noise is set to zero. Black dots show $500$ Monte Carlo endpoints in the $(p_{x},p_{y})$ plane, while the filled green marker indicates the mean endpoint. The blue curves represent projected Lie-group covariance contours (principal great circles of the $3\sigma$ covariance ellipsoid in the Lie algebra, mapped to the group and projected onto the $(p_{x},p_{y})$ plane). The dashed orange curve is the fitted Euclidean $3\sigma$ covariance ellipse obtained from the sample covariance of the endpoints in $\mathbb{R}^{2}$ .

Figure 10: Batch propagation between keyframes. The figure illustrates the propagation of the extended state

(\hat{X}_{i},\Sigma_{i})

through high-rate IMU integration to obtain the keyframe estimate

(\hat{X}_{j},\Sigma_{j})

. Equations ( 45 ) and ( 54 ) summarize the second-order propagation of the mean and covariance over the interval

[t_{i},t_{j}]

, accounting for all IMU measurements between the two keyframes.

Sensor	Measurement	Processed Output $y_{k}$	Jacobian $H_{k}^{\star}$
GPS position with fixed lever arm $b$	$p_{gps}=p+Rb$	( 66 ) with $r=(b,0,1)$	$H_{k}^{\ell}=[-p_{gps}^{\wedge}\;\;~0~\;\;I]$
GPS velocity with fixed lever arm $b$	$v_{gps}=v+R(\omega\times b)$	( 66 ) with $r=(\omega\times b,1,0)$	$H_{k}^{\ell}=[-v_{gps}^{\wedge}\;\;~I~\;\;0]$
Magnetometer	$R^{\top}r_{m}$	( 64 ) with $r=(r_{m},0,0)$	$H^{r}=[-r_{m}^{\wedge}\;\;~0~\;\;0]$
Landmark position	$R^{\top}(p_{\ell}-p)$	( 64 ) with $r=(p_{\ell},0,1)$	$H^{r}=[-p_{\ell}^{\wedge}\;\;~0~\;\;I]$
DVL	$R^{\top}v$	( 64 ) with $r=(0_{3\times 1},-1,0)$	$H^{r}=[0\;\;~-I~\;\;0]$
Barometer (altitude)	$e_{3}^{\top}p$	Same	$H_{k}=[e_{3}^{\top}\hat{p}^{\wedge}\;\;\;~0~-e_{3}^{\top}\,]$

Table 1: Jacobians for common aiding sensors. All Jacobians are expressed with respect to the right-invariant error. For non-invariant measurement models, the generic Jacobian

H_{k}

is used directly together with the corresponding measurement noise covariance

V_{k}

. For right- (resp. left-)invariant measurement models, the corresponding invariant Jacobian

H_{k}^{r}

(resp.

H_{k}^{\ell}

) is used, provided that the associated noise covariance is consistently transformed (denoted

V_{k}^{\star}

when paired with

H_{k}^{\star}

). In all cases, the noise covariance must correspond to the processed output.

Symbol	Definition	Description
$D$	$\begin{bmatrix}0_{3\times 3}&0_{3\times 2}\\[2.0pt] 0_{2\times 3}&S\end{bmatrix}$	Velocity–position coupling in the homogeneous model.
$S$	$\begin{bmatrix}0&1\\[2.0pt] 0&0\end{bmatrix}$	Velocity–position coupling in the Euclidean model.
$\bar{D}$	$(e_{3}e_{2}^{\top})\!\otimes\!I_{3}$	Velocity–position coupling in the Lie–algebra coordinates.
$\Pi_{n}^{m}$	$\big[\,I_{m}\;\;0_{m\times(n-m)}\,\big]$	Projection extracting the first $m$ –components.

Table 2: Constant matrices used throughout the paper. For convenience, we collect here the constant matrices that appear throughout the paper. They are independent of the state and time, and are used to compactly express the group operations, adjoint mappings, and linearized dynamics on

\mathbb{SE}_{2}(3)

Figure 11: Comparison of the multiplicative EKF and the invariant EKF for GPS-aided inertial navigation, with and without magnetometer aiding. The filters are propagated using IMU measurements at $100\,\mathrm{Hz}$ and corrected whenever aiding measurements are available. Error panels show the first $10\,\mathrm{s}$ , with median curves and shaded 10th–90th percentile bands over 50 paired noise realizations. Trajectory panels show the full $60\,\mathrm{s}$ trajectory for one realization, with ground truth in black. The top row corresponds to IMU and GPS position updates only (no magnetometer), whereas the bottom row additionally incorporates body-frame magnetometer measurements providing heading information at $20\,\mathrm{Hz}$ . In the absence of magnetometer aiding, the invariant EKF exhibits smaller median attitude and velocity errors during the initial transient, while the difference in position error is more modest. Including magnetometer measurements improves the performance of both filters; however, the invariant formulation retains an advantage during the initial transient, particularly in attitude estimation.

Sensor	Measurement	Output $y_{i}$	Matrix $H_{i}$	Vector $r_{i}^{\top}$
GPS position (lever arm $b$ )	$p_{\mathrm{gps}}=p+Rb$	$b=R^{\top}(p_{\mathrm{gps}}-p)$	$I_{3}$	$\left[\begin{array}[]{ccc}0&-1&p_{\mathrm{gps}}^{\top}\end{array}\right]$
Landmarks	$y=R^{\top}(p_{\ell}-p)$	$y=R^{\top}(p_{\ell}-p)$	$I_{3}$	$\left[\begin{array}[]{ccc}0&-1&p_{\ell}^{\top}\end{array}\right]$
Bearing to landmark	$b_{\ell}=\frac{R^{\top}(p_{\ell}-p)}{\\|p_{\ell}-p\\|}$	$0=\pi_{b_{\ell}}R^{\top}(p_{\ell}-p)$	$\pi_{b_{\ell}}=I_{3}-b_{\ell}b_{\ell}^{\top}$	$\left[\begin{array}[]{ccc}0&-1&p_{\ell}^{\top}\end{array}\right]$
Tilt-like constraint	$y=b^{\top}R^{\top}a$	$y=b^{\top}R^{\top}a$	$b^{\top}$	$\left[\begin{array}[]{ccc}0&0&a^{\top}\end{array}\right]$
Magnetometer	$y=R^{\top}a$	$y=R^{\top}a$	$I_{3}$	$\left[\begin{array}[]{ccc}0&0&a^{\top}\end{array}\right]$
Pitot tube	$y=e_{1}^{\top}R^{\top}(v-v_{w})$	$y=e_{1}^{\top}R^{\top}(v-v_{w})$	$e_{1}^{\top}$	$\left[\begin{array}[]{ccc}1&0&-v_{w}^{\top}\end{array}\right]$

Table 3: Measurements compatible with the

\mathbb{SE}_{5}(3)

linear error model. All outputs admit an innovation that has the exact linear form

z_{i}=y_{i}-\hat{y}_{i}=C_{i}\,\xi

with

C_{i}=-(r_{i}^{\top}\otimes H_{i})

. Note the difference between the actual measurement and the processed output

y_{i}

used in the filter.

Equivariant Filtering

See all 8 papers

Jul 3, 2026

Derivations of Error-State Kalman Filter Kinematics for Globally Applicable Aided Inertial Navigation Systems

Global navigation systems require state estimation algorithms that handle Earth's curvature, Earth's rotation, and gravitational variations. These factors can typically be neglected in local navigation algorithms for robots, drones, etc. In classical error-state Kalman Filtering (ESKF) the error state dynamics are trajectory-dependent. Invariant ESKFs utilize Lie Group symmetries to represent the error, which can render error propagation trajectory-independent for group-affine systems. Choosing between a standard filter (where position and velocity errors are defined additively in the navigation frame), a left-invariant filter (where errors are represented in the body frame) and a right-invariant filter (where errors are represented in the navigation/world frame) depends on system dynamics and sensor configuration. This note presents the mathematical formulas for four classical and invariant ESKFs for globally applicable aided inertial navigation systems. It is intended to serve as a systematic reference for comparison and implementation.

Antonia Hager, Torleiv H. Bryne

Airbus Central Research and Technology, Taufkirchen, Germany · Department of Engineering Cybernetics, Norwegian University of Science and Technology, Trondheim, Norway

Kalman Filtering Error-State Kalman Filtering Lie Group Methods Invariant EKF Inertial Navigation

Ask PDF

Aug 30, 2026

A Sliding Window Filter on the Galilean Group for Consistent Aided Inertial Navigation with Unknown Measurement Delays

We study aided inertial navigation when the aiding sensor measurements are subject to an unknown constant delay. The goal is to estimate the delay and navigation state jointly so that delayed measurements correct the trajectory at the appropriate times, yielding a more accurate navigation solution. We formulate the problem on the special Galilean group, which provides a natural state-space structure for aided navigation with uncertainty in both motion and timing. We then examine the observability of joint delay and state estimation and show that, for a single delayed measurement, the model admits an exact symmetry in which a change in the delay can be compensated by a change in the navigation state, leaving the measurement unchanged. Processing measurements individually allows spurious information to `leak' along the corresponding null direction of the measurement Jacobian, producing overconfident and inconsistent estimates. Applying measurements from multiple times together can eliminate this direction when the trajectory is informative enough. Motivated by this result, we develop a sliding window filter that retains a short history of navigation states and applies delayed aiding corrections jointly across the active window. We conduct a series of simulation studies to characterize estimator accuracy and consistency. The simulations demonstrate that an estimator that does not maintain an adequate window can rapidly become highly inconsistent, whereas even a short sliding window markedly improves consistency by providing the temporal support that observability requires.

Jonathan Kelly

Space & Terrestrial Autonomous Robotic Systems (STARS) Laboratory, University of Toronto Institute for Aerospace Studies (UTIAS), Toronto, Canada.

Inertial Navigation

Ask PDF

Jun 28, 2026

On the Identifiability of Aided Inertial Navigation Under Measurement Delays: A Geometric Approach

In aided inertial navigation, measurements from different sensors are often subject to unknown relative time delays. Consider a single aiding sensor whose measurements have an unknown but constant delay relative to the inertial-measurement data stream. We study the identifiability of the delay and the initial navigation state that parameterizes the trajectory. Identifiability depends on both the temporal structure of the aiding measurements and the form of the trajectory itself. Our geometric analysis shows that, for a larger class of uninformative (i.e., degenerate) trajectories than has previously been reported, the delayed measurement model admits a continuous symmetry that prevents unique delay-and-state recovery.

Jonathan Kelly

Parameter Identifiability Inertial Navigation

Ask PDF

Abstract

Equivariant Filtering

Explore similar work

Derivations of Error-State Kalman Filter Kinematics for Globally Applicable Aided Inertial Navigation Systems

A Sliding Window Filter on the Galilean Group for Consistent Aided Inertial Navigation with Unknown Measurement Delays

On the Identifiability of Aided Inertial Navigation Under Measurement Delays: A Geometric Approach