MagFactor Family - GTSAM Docs

Overview¶

The MagFactor family provides factors for incorporating magnetometer measurements, which sense the Earth’s magnetic field, into a GTSAM factor graph. These factors are primarily used to constrain the orientation (mostly “yaw”) of a body or sensor.

Magnetometers measure the local magnetic field vector in the sensor’s own frame. The factors relate this measurement to the known (or estimated) magnetic field direction in the navigation frame via the body’s rotation.

Several variants exist depending on what is known and what is being estimated:

Measurement Model¶

The general model assumed by these factors is:

bM_{measured} = R_{bn} \cdot (s \cdot \hat{d}_n) + b

(1)

where:

$bM_{measured}$ : The 3D magnetic field vector measured by the sensor, in the body frame.
$R_{bn}$ : The rotation matrix from the navigation frame ( $n$ ) to the body frame ( $b$ ). This is the inverse of the rotation usually stored in Pose3 or NavState ( $R_{nb}$ ).
$s$ : A scale factor relating the magnetic field strength to the magnetometer’s output units (e.g., nT).
$\hat{d}_n$ : The unit vector representing the direction of the Earth’s magnetic field in the navigation frame.
$b$ : A 3D additive bias vector in the body frame.

The factor error is typically calculated as:

e = bM_{measured} - [ R_{bn} \cdot (s \cdot \hat{d}_n) + b ]

(2)

Different factors treat different components ( $R_{bn}$ , $s$ , $\hat{d}_n$ , $b$ ) as known constants or as variables to be estimated.

Factor Variants¶

MagFactor:
- Estimates: Rot2 (intended for yaw $R_{nb}$ ).
- Assumes Known: scale, direction, bias.
- Note: Uses Rot2 which might be limiting unless the sensor is always level.
MagFactor1:
- Estimates: Rot3 ( $R_{nb}$ ).
- Assumes Known: scale, direction, bias.
- This is often the most practical factor for direct orientation estimation when calibration is known.
MagFactor2:
- Estimates: nM (Point3, the scaled field vector $s \cdot \hat{d}_n$ in nav frame), bias (Point3).
- Assumes Known: Rot3 ( $R_{nb}$ ).
- Useful for calibrating the local field and bias if orientation is known (e.g., from other sensors).
MagFactor3:
- Estimates: scale (double), direction (Unit3), bias (Point3).
- Assumes Known: Rot3 ( $R_{nb}$ ).
- Provides full calibration of scale, direction, and bias when orientation is known.
MagPoseFactor<POSE>: (Separate Header: MagPoseFactor.h)
- Estimates: Pose2 or Pose3.
- Assumes Known: scale, direction, bias, optional body_P_sensor.
- Similar to MagFactor1 but works directly on Pose types and handles sensor-to-body transforms.

Key Functionality / API (MagFactor1 Example)¶

Constructor: MagFactor1(key, measured, scale, direction, bias, model): Creates the factor connecting the Rot3 key, given the measurement, known calibration parameters, and noise model.
evaluateError(nRb): Calculates the 3D error vector $e$ based on the current Rot3 estimate $R_{nb}$ .

Usage Example (MagFactor1)¶

Using a magnetometer to help estimate a Rot3 orientation, assuming known calibration.

import gtsam
import numpy as np
from gtsam.symbol_shorthand import R # Rotation key

# --- Assumed Known Calibration & Field ---
# Local magnetic field direction in navigation frame (e.g., NED)
# Example: From NOAA for specific location/date
# Field Strength: 48343.4 nT
# Declination: -4.94 deg -> Angle from North towards West
# Inclination: 62.78 deg -> Angle below horizontal
declination_rad = np.deg2rad(-4.94)
inclination_rad = np.deg2rad(62.78)
field_strength_nT = 48343.4

# Convert Dec/Inc to NED vector components
north_comp = np.cos(inclination_rad) * np.cos(declination_rad)
east_comp = np.cos(inclination_rad) * np.sin(declination_rad)
down_comp = np.sin(inclination_rad)
n_direction = gtsam.Unit3(np.array([north_comp, east_comp, down_comp]))

# Assume scale factor converts unit vector to nT (can be absorbed if field strength is used)
mag_scale = field_strength_nT

# Assume known magnetometer bias in body frame (nT)
mag_bias_body = gtsam.Point3(10.0, -5.0, 2.0) 

# --- Simulation: Generate Measurement ---
# Assume a ground truth rotation (e.g., 30 deg yaw right in NED)
truth_nRb = gtsam.Rot3.Yaw(np.deg2rad(30)) 
truth_bRn = truth_nRb.inverse()

# Calculate the expected magnetic field in the body frame (ideal, before bias)
n_field_vector = mag_scale * n_direction.point3()
b_field_ideal = truth_bRn.rotate(n_field_vector)

# Calculate the measured value including bias
b_measured = b_field_ideal + mag_bias_body

# --- Factor Creation ---
rot_key = R(0)

# Noise model for the magnetometer measurement (nT)
mag_noise_sigma = 50.0 # nT
noise_model = gtsam.noiseModel.Isotropic.Sigma(3, mag_noise_sigma)

# Create MagFactor1
mag_factor = gtsam.MagFactor1(rot_key, b_measured, mag_scale, 
                                n_direction, mag_bias_body, noise_model)

print("Created MagFactor1:")
mag_factor.print()

# --- Evaluate Error ---
# Evaluate at the ground truth rotation (error should be zero)
error_at_truth = mag_factor.evaluateError(truth_nRb)
print("\nError at ground truth rotation (should be zero):", error_at_truth)

# Evaluate at a different rotation (error should be non-zero)
test_nRb = gtsam.Rot3.Yaw(np.deg2rad(25)) # Slightly off
error_at_test = mag_factor.evaluateError(test_nRb)
print("Error at test rotation:", error_at_test)

Created MagFactor1:
  keys = { r0 }
isotropic dim=3 sigma=50

Error at ground truth rotation (should be zero): [0. 0. 0.]
Error at test rotation: [1034.79105955 1628.05638524    0.        ]

Important Notes¶

Coordinate Frames: Be very careful with navigation frame (NED vs ENU) and body frame conventions. Ensure the direction vector and the Rot3/Pose3 variables use the same navigation frame. The measured and bias are typically in the body frame.
Units: Ensure consistency between the scale, bias, measured values and the noise model sigma (e.g., all in nanoTesla (nT)).
Calibration: Accurate knowledge of scale, direction, and bias is crucial for MagFactor1 and MagPoseFactor. If these are unknown, consider using MagFactor2/MagFactor3 or online calibration techniques.

Source¶

MagFactor.h