AttitudeFactor

In GPS-denied environments, orientation often has to be stabilized from inertial or magnetic measurements:

Gravity from an accelerometer can constrain roll and pitch.
Magnetic field measurements can constrain yaw, with the usual caution about magnetic disturbances.

The AttitudeFactor family in GTSAM constrains orientation from directional measurements. In C++, the factor is instantiated as AttitudeFactor<Rot3>, AttitudeFactor<Pose3>, AttitudeFactor<NavState>, AttitudeFactor<Gal3>, AttitudeFactor<Se23>, or AttitudeFactor<ExtendedPose3d>. In the Python wrapper, these appear through the normal template wrapper mechanism as AttitudeFactorRot3, AttitudeFactorPose3, AttitudeFactorNavState, AttitudeFactorGal3, AttitudeFactorSe23, and AttitudeFactorExtendedPose3d.

This gives the optimizer an absolute directional reference that can significantly improve navigation accuracy and reduce attitude drift.

import numpy as np

from gtsam import AttitudeFactorPose3, Pose3, Rot3, Unit3
from gtsam.noiseModel import Isotropic
from gtsam.symbol_shorthand import X

Mathematical Foundation¶

Concept¶

The AttitudeFactor constrains the rotation $R_{nb}$ from body frame $b$ to navigation frame $n$ so that a known navigation-frame reference direction aligns with a measured body-frame direction after rotation:

\text{nRef} \approx R_{nb} \cdot \text{bMeasured}

(1)

where:

nRef is the known direction in the navigation frame, for example gravity.
bMeasured is the measured direction in the body frame, for example an accelerometer or magnetometer direction, typically represented as a Unit3.
$R_{nb}$ is the orientation that rotates body-frame directions into the navigation frame.

Error Function¶

The factor computes a 2D directional error between nRef and the rotated measurement:

\mathbf{e} = \mathrm{error}(\text{nRef}, R_{nb} \cdot \text{bMeasured})

(2)

The residual is zero when the rotated body-frame measurement aligns with the reference direction.

Jacobians¶

For optimization, GTSAM computes the $2 \times 3$ Jacobian of this directional error with respect to the underlying rotation coordinates. For state types such as Pose3, NavState, Gal3, Se23, and ExtendedPose3d, the factor applies the chain rule through each state’s rotation(...) method so the Jacobians with respect to the full state are correct.

A practical caveat is that this directional residual becomes weak 180 degrees away from the correct direction, so the factor works best when the state is initialized in the correct hemisphere.

Available Variants¶

C++ specializations (tested to work, you can template on other types, of course):

AttitudeFactor<Rot3>
AttitudeFactor<Pose3>
AttitudeFactor<NavState>
AttitudeFactor<Gal3>
AttitudeFactor<Se23>
AttitudeFactor<ExtendedPose3d>

AttitudeFactor<Pose3> and the other state-based variants automatically extract the state’s rotational part and account for the chain rule in their Jacobians.

Python wrapper class names:

AttitudeFactorRot3
AttitudeFactorPose3
AttitudeFactorNavState
AttitudeFactorGal3
AttitudeFactorSe23
AttitudeFactorExtendedPose3d

C++ Usage¶

Include the factor header:

#include <gtsam/navigation/AttitudeFactor.h>

Example for a Rot3 variable:

gtsam::Key rotKey = ...;
gtsam::Unit3 nRef(0, 0, -1);
gtsam::Unit3 bMeasured(0, 0, 9.8);
auto noiseModel = gtsam::noiseModel::Isotropic::Sigma(2, 0.1);

gtsam::NonlinearFactorGraph graph;
graph.emplace_shared<gtsam::AttitudeFactor<gtsam::Rot3>>(
    rotKey, nRef, noiseModel, bMeasured);

Example for a Pose3 variable:

gtsam::Key poseKey = ...;
gtsam::Unit3 nRef(0, 0, -1);
gtsam::Unit3 bMeasured(0, 0, 9.8);
auto noiseModel = gtsam::noiseModel::Isotropic::Sigma(2, 0.1);

gtsam::NonlinearFactorGraph graph;
graph.emplace_shared<gtsam::AttitudeFactor<gtsam::Pose3>>(
    poseKey, nRef, noiseModel, bMeasured);

Key parameters:

key: the variable key in the factor graph.
nRef: the known navigation-frame direction.
bMeasured: the measured body-frame direction. It defaults to the positive body Z-axis if omitted.
noiseModel: a 2D directional noise model.

Python Example¶

The Python wrapper exposes the Pose3 specialization as AttitudeFactorPose3.

# Known reference direction in the navigation frame (ENU, Z is up).
nRef_gravity = Unit3(np.array([0.0, 0.0, -1.0]))

# Accelerometer direction measured in the body frame.
bMeasured_acc = Unit3(np.array([0.1, 0.0, -9.8]))

# 2D directional noise model.
noise_model = Isotropic.Sigma(2, 0.1)

# Create the factor.
attitude_factor = AttitudeFactorPose3(
    X(0), nRef_gravity, noise_model, bMeasured_acc
)

print("Created AttitudeFactorPose3:")
attitude_factor.print()

# Evaluate the residual at the identity pose.
pose = Pose3(Rot3(), np.array([0.0, 0.0, 0.0]))
attitude_factor.evaluateError(pose)

Created AttitudeFactorPose3:
AttitudeFactorPose3 on x0
  reference direction in nav frame: : 0
 0
-1
  measured direction in body frame: :0.0102036
        0
-0.999948
isotropic dim=2 sigma=0.1

array([ 0. , -0.01020355])