AHRSFactor

Overview¶

The AHRSFactor class implements a factor to implement an Attitude and Heading Reference System (AHRS) within GTSAM. It is a binary factor, taking preintegrated measurements from a gyroscope between two discrete time steps, typically denoted as $t_i$ and $t_j$ . These preintegrated measurements encapsulate the rotational motion observed by an inertial measurement unit (IMU) between these two timestamps.

The AHRSFactor thus constrains two attitude states (represented as elements of $SO(3)$ ) based solely on gyroscope measurements. Accelerometer or magnetometer aiding, needed to build a complete AHRS system, must be handled separately.

%pip install --quiet gtsam-develop plotly

Note: you may need to restart the kernel to use updated packages.

Mathematical Formulation¶

The AHRSFactor relies on the use of PreintegratedRotation, which applies successive exponential maps to each individual gyroscope measurement ω over the interval $[t_i, t_j]$ . In this approach, every measurement contributes a small rotation given by $\exp(\omega_k \Delta t)$ , and the overall preintegrated rotation is obtained by composing these incremental rotations:

\Delta R_{ij}^{meas} = \prod_{k} \exp(\omega_k \Delta t)

(1)

Given two estimated rotations $R_i$ at time $t_i$ and $R_j$ at time $t_j$ , the factor enforces:

R_j \approx R_i \cdot \Delta R_{ij}^{meas}

(2)

The error term optimized by the factor graph is the rotational discrepancy captured by the logarithmic map:

e = \text{Log}\left((\Delta R_{ij}^{meas})^T \cdot R_i^T R_j\right)

(3)

Note: the reality is a bit more complicated, because the code also takes into account the effects of bias, and if desired, coriolis forces.

Key Functionality¶

Constructor¶

The constructor initializes the factor using a preintegrated AHRS measurement object, relating orientation states at discrete time instances $t_i$ and $t_j$ .

Core Methods¶

evaluateError: calculates the error between two estimated orientations at times $t_i$ and $t_j$ , relative to the preintegrated gyroscopic measurements. The error is computed as:
$\text{error} = \text{Log}\left((\Delta R_{ij}^{meas})^T \cdot R_i^T R_j\right)$
(4)
Here:
- $R_i, R_j$ are the estimated rotation matrices at times $t_i$ and $t_j$ .
- $\Delta R_{ij}^{meas}$ is the rotation matrix obtained by integrating gyroscope measurements from $t_i$ to $t_j$ .
- $\text{Log}(\cdot)$ is the logarithmic map from $SO(3)$ to $\mathbb{R}^3$ .
The resulting 3-dimensional error vector represents the rotational discrepancy.
print: outputs a readable representation of the internal state of the factor, including the associated time steps and preintegrated measurements, aiding debugging and verification.
equals determines if another AHRSFactor instance is identical, useful in testing scenarios or when verifying the correctness of factor graph constructions.

Usage¶

First, create a PreintegratedAhrsMeasurements (PAM) object by supplying the necessary IMU parameters.

import numpy as np
from gtsam import PreintegrationParams, PreintegratedAhrsMeasurements

params = PreintegrationParams.MakeSharedU(9.81)
params.setGyroscopeCovariance(np.deg2rad(1)*np.eye(3))
params.setAccelerometerCovariance(0.01*np.eye(3))

biasHat = np.zeros(3)  # Assuming no bias for simplicity
pam = PreintegratedAhrsMeasurements(params, biasHat)

Next, integrate each gyroscope measurement along with its corresponding time interval to accumulate the preintegrated rotation.

from gtsam import Point3
np.random.seed(42)  # For reproducibility
for _ in range(15):  # Add 15 random measurements, biased to move around z-axis
    omega = Point3(0,0,-0.5) + 0.1*np.random.randn(3)  # Random angular velocity vector
    pam.integrateMeasurement(omega, deltaT=0.1)

Finally, construct the AHRSFactor using the accumulated PAM and the keys representing the rotation states at the two time instants.

from gtsam import AHRSFactor
bias_key = 0  # Key for the bias
i, j = 1, 2  # Indices of the two attitude unknowns
ahrs_factor = AHRSFactor(i, j, bias_key, pam)

print(ahrs_factor)

AHRSFactor(1,2,0,  preintegrated measurements:    deltaTij [1.5]
    deltaRij.ypr = (  -0.82321 -0.0142842  0.0228577)
biasHat [0 0 0]
 PreintMeasCov [   0.0261799 1.73472e-18 1.35525e-20
1.73472e-18   0.0261799 9.23266e-20
1.35525e-20 1.17738e-19   0.0261799 ]
  noise model: diagonal sigmas [0.161802159; 0.161802159; 0.161802159];