cs.ROMar 7, 2026
SaveTutorial on Aided Inertial Navigation Systems: A Modern Treatment Using Lie-Group Theoretical Methods
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 is defined by the basis . The body-fixed IMU frame has basis vectors . The vehicle position expressed in is , and its orientation is represented by the rotation matrix , which maps body-frame vectors into the inertial frame.
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 , , and axes.
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 , , and body-frame directions.
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 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 for multiple sampling intervals , where the circular trajectory is preserved independently of . The right panel shows the classical INS discretization ( , ), for which discretization errors accumulate as 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 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 . The mean estimate 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 -axis. For each mean pose, approximately Monte Carlo samples are drawn in the Lie algebra from a zero-mean Gaussian distribution with standard deviations on the yaw– – components and mapped to the group. The position components are shown in the – plane, while the yaw component is encoded by the orientation of each line segment. Left: direct-product error model on [ forster2016manifold ] , which produces ellipsoidal position uncertainty independent of the distance from the origin. Right: right-invariant error model on , 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 term in ( 37 ).
Figure 9: Simple propagation on with , , sampling period , and propagation duration . The gyroscope is corrupted by discrete-time noise along the -axis with standard deviation per sample, while accelerometer noise is set to zero. Black dots show Monte Carlo endpoints in the plane, while the filled green marker indicates the mean endpoint. The blue curves represent projected Lie-group covariance contours (principal great circles of the covariance ellipsoid in the Lie algebra, mapped to the group and projected onto the plane). The dashed orange curve is the fitted Euclidean covariance ellipse obtained from the sample covariance of the endpoints in .
Figure 10: Batch propagation between keyframes. The figure illustrates the propagation of the extended state through high-rate IMU integration to obtain the keyframe estimate . Equations ( 45 ) and ( 54 ) summarize the second-order propagation of the mean and covariance over the interval , accounting for all IMU measurements between the two keyframes.
| Sensor | Measurement | Processed Output | Jacobian |
|---|---|---|---|
| GPS position with fixed lever arm | ( 66 ) with | ||
| GPS velocity with fixed lever arm | ( 66 ) with | ||
| Magnetometer | ( 64 ) with | ||
| Landmark position | ( 64 ) with | ||
| DVL | ( 64 ) with | ||
| Barometer (altitude) | Same |
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 is used directly together with the corresponding measurement noise covariance . For right- (resp. left-)invariant measurement models, the corresponding invariant Jacobian (resp. ) is used, provided that the associated noise covariance is consistently transformed (denoted when paired with ). In all cases, the noise covariance must correspond to the processed output.
| Symbol | Definition | Description |
|---|---|---|
| Velocity–position coupling in the homogeneous model. | ||
| Velocity–position coupling in the Euclidean model. | ||
| Velocity–position coupling in the Lie–algebra coordinates. | ||
| Projection extracting the first –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 .
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 and corrected whenever aiding measurements are available. Error panels show the first , with median curves and shaded 10th–90th percentile bands over 50 paired noise realizations. Trajectory panels show the full 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 . 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 | Matrix | Vector |
|---|---|---|---|---|
| GPS position (lever arm ) | \left[\begin{array}[]{ccc}0&-1&p_{\mathrm{gps}}^{\top}\end{array}\right] | |||
| Landmarks | \left[\begin{array}[]{ccc}0&-1&p_{\ell}^{\top}\end{array}\right] | |||
| Bearing to landmark | \left[\begin{array}[]{ccc}0&-1&p_{\ell}^{\top}\end{array}\right] | |||
| Tilt-like constraint | \left[\begin{array}[]{ccc}0&0&a^{\top}\end{array}\right] | |||
| Magnetometer | \left[\begin{array}[]{ccc}0&0&a^{\top}\end{array}\right] | |||
| Pitot tube | \left[\begin{array}[]{ccc}1&0&-v_{w}^{\top}\end{array}\right] |
Table 3: Measurements compatible with the linear error model. All outputs admit an innovation that has the exact linear form with . Note the difference between the actual measurement and the processed output used in the filter.