GPSFactor Family

Overview¶

The GPSFactor family provides factors for incorporating Global Positioning System (GPS) measurements into a GTSAM factor graph. GPS typically provides measurements of position in Latitude/Longitude/Height. These GPS factors, however, assume the GPS measurement has been converted into a local Cartesian navigation frame (e.g., ENU, NED, or ECEF).

Different variants exist to handle:

State type: Pose3 or NavState.
Lever arm: Whether the GPS antenna is offset from the body frame origin.
Calibration: Whether the lever arm itself is being estimated.

Factor Variants¶

For Pose3 states:

GPSFactor:
- Connects to: Pose3 key.
- Assumes: Zero lever arm (GPS measurement corresponds directly to the Pose3 origin).
- Measurement: 3D position (Point3) in the navigation frame.
- Error: $p_{pose} - p_{measured}$
GPSFactorArm:
- Connects to: Pose3 key.
- Assumes: Known, fixed lever arm (bL vector in the body frame).
- Measurement: 3D position (Point3) in the navigation frame.
- Error: $(p_{pose} + R_{nb} \cdot bL) - p_{measured}$
GPSFactorArmCalib:
- Connects to: Pose3 key, Point3 key (for the lever arm).
- Assumes: Lever arm (bL) is unknown and estimated.
- Measurement: 3D position (Point3) in the navigation frame.
- Error: $(p_{pose} + R_{nb} \cdot bL_{estimated}) - p_{measured}$

For NavState states:

GPSFactor2: Like GPSFactor but connects to a NavState key.
GPSFactor2Arm: Like GPSFactorArm but connects to a NavState key.
GPSFactor2ArmCalib: Like GPSFactorArmCalib but connects to a NavState key and a Point3 lever arm key.

(The ‘2’ suffix historically denoted factors using NavState).

Mathematical Formulation (GPSFactorArm Example)¶

Let:

$T_{nb} = (R_{nb}, p_{nb})$ be the Pose3 state (body frame $b$ in navigation frame $n$ ).
$L_b$ be the known lever arm vector from the body origin to the GPS antenna, expressed in the body frame.
$p_{gps}$ be the measured GPS position in the navigation frame.

The predicted position of the GPS antenna in the navigation frame is:

p_{ant, pred} = p_{nb} + R_{nb} \cdot L_b

(1)

The factor’s 3D error vector is the difference between the predicted antenna position and the measured GPS position:

e = p_{ant, pred} - p_{gps}

(2)

The noise model reflects the uncertainty of the $p_{gps}$ measurement in the navigation frame.

Key Functionality / API (Common Patterns)¶

Constructor: Takes the relevant key(s), the measured Point3 position gpsIn (in nav frame), the noise model, and potentially the leverArm (Point3 in body frame).
evaluateError(...): Calculates the 3D error vector based on the connected state variable(s) and the measurement.
measurementIn(): Returns the stored Point3 measurement.
leverArm() (For Arm variants): Returns the stored Point3 lever arm.

Usage Example (GPSFactor and GPSFactorArm)¶

Assume we have a GPS reading converted to a local ENU frame.

import gtsam
import numpy as np
from gtsam.symbol_shorthand import X, L # Pose key, Lever arm key

# --- Setup ---
pose_key = X(0)

# GPS Measurement in local ENU frame (meters)
gps_measurement_enu = gtsam.Point3(10.5, 20.2, 5.1)

# Noise model for GPS measurement (e.g., 0.5m horizontal, 1.0m vertical sigma)
gps_sigmas = np.array([0.5, 0.5, 1.0])
gps_noise_model = gtsam.noiseModel.Diagonal.Sigmas(gps_sigmas)

# --- Scenario 1: GPSFactor (Zero Lever Arm) ---
gps_factor_zero_arm = gtsam.GPSFactor(pose_key, gps_measurement_enu, gps_noise_model)
print("Created GPSFactor (zero lever arm):")
gps_factor_zero_arm.print()

# Evaluate error: Error is difference between pose translation and measurement
test_pose1 = gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(10.0, 20.0, 5.0))
error1 = gps_factor_zero_arm.evaluateError(test_pose1)
print("\nGPSFactor Error:", error1) # Expected: [0.5, 0.2, 0.1]

# --- Scenario 2: GPSFactorArm (Known Lever Arm) ---
# Assume antenna is 10cm behind and 5cm above the body origin
lever_arm_body = gtsam.Point3(-0.1, 0.0, 0.05) 

gps_factor_with_arm = gtsam.GPSFactorArm(pose_key, gps_measurement_enu, 
                                         lever_arm_body, gps_noise_model)
print("\nCreated GPSFactorArm:")
gps_factor_with_arm.print()

# Evaluate error: Error is difference between (pose + R*lever_arm) and measurement
# Use the same test pose as before
predicted_antenna_pos = test_pose1.transformFrom(lever_arm_body)
error2 = gps_factor_with_arm.evaluateError(test_pose1)
print("\nGPSFactorArm Error:", error2) 
print(" ( Pose:      ", test_pose1.translation() , ")")
print(" ( Lever Arm: ", lever_arm_body, ")")
print(" ( Predicted Antenna Pos: ", predicted_antenna_pos, ")")
print(" ( Measured GPS Pos:    ", gps_measurement_enu, ")")
# Expected: predicted_antenna_pos - gps_measurement_enu 
#           = [9.9, 20.0, 5.05] - [10.5, 20.2, 5.1] = [-0.6, -0.2, -0.05]

# --- Scenario 3: GPSFactorArmCalib (Example Setup - Not Evaluated) ---
lever_arm_key = L(0) # Key for the unknown lever arm
gps_factor_calib = gtsam.GPSFactorArmCalib(pose_key, lever_arm_key, 
                                            gps_measurement_enu, gps_noise_model)
print("\nCreated GPSFactorArmCalib:")
gps_factor_calib.print()

Created GPSFactor (zero lever arm):
GPSFactor on x0
  GPS measurement: 10.5
20.2
 5.1
  noise model: diagonal sigmas [0.5; 0.5; 1];

GPSFactor Error: [-0.5 -0.2 -0.1]

Created GPSFactorArm:
GPSFactorArm on x0
  GPS measurement: 10.5 20.2  5.1
  Lever arm: -0.1    0 0.05
  noise model: diagonal sigmas [0.5; 0.5; 1];

GPSFactorArm Error: [-0.6  -0.2  -0.05]
 ( Pose:       [10. 20.  5.] )
 ( Lever Arm:  [-0.1   0.    0.05] )
 ( Predicted Antenna Pos:  [ 9.9  20.    5.05] )
 ( Measured GPS Pos:     [10.5 20.2  5.1] )

Created GPSFactorArmCalib:
GPSFactorArmCalib on x0
  GPS measurement: 10.5 20.2  5.1
  noise model: diagonal sigmas [0.5; 0.5; 1];

Coordinate Frames¶

It’s crucial to ensure consistency in coordinate frames:

GPS Measurement (gpsIn): Must be provided in the chosen local Cartesian navigation frame (e.g., ENU, NED).
Lever Arm (leverArm): Must be provided in the body frame.
Pose/NavState Variables: Represent the pose of the body frame in the navigation frame ( $T_{nb}$ ).