DopplerFactor and DopplerFactorArm incorporate GNSS range-rate measurements into a GTSAM factor graph. The base factor estimates receiver velocity and the change in receiver clock bias between adjacent epochs. The lever-arm variant additionally accounts for antenna motion caused by body rotation, and it can accept a local navigation-frame pose and velocity.
import gtsam
import numpy as np
C_LIGHT = 299792458.0 # Speed of light [m/s].
LAMBDA_L1 = 0.190293672798365 # GPS L1 wavelength [m/cycle].
satellite_position = np.array([-5824269.46, -22935011.27, -12195522.22])
receiver_position = np.array([-3961908.12, 3348995.59, 3698211.13])
satellite_velocity = np.array([-1200.0, 2400.0, 800.0])
receiver_velocity = np.array([0.3, -0.1, 0.05])
satellite_clock_drift = 1.2e-9 # [s/s].
receiver_clock_drift = 4.5e-9 # [s/s].
dt = 0.2 # Time between consecutive receiver states [s].
bias_previous = 1.0e-6 # Receiver clock bias at epoch k-1 [s].
bias_current = bias_previous + receiver_clock_drift * dt
noise_model = gtsam.noiseModel.Unit.Create(1)DopplerFactor¶
The factor uses keys [velocity, clock_bias_previous, clock_bias_current]. Its residual is the predicted range rate minus the measured range rate (-wavelength * Doppler), including the satellite clock drift and the Earth-rotation (Sagnac) correction:
doppler_factor = gtsam.DopplerFactor(
0, 1, 2,
-1500.0, # Measured Doppler [Hz].
LAMBDA_L1, satellite_position, satellite_velocity, receiver_position,
dt, satellite_clock_drift, noise_model
)
residual = doppler_factor.evaluateError(
receiver_velocity, bias_previous, bias_current
)[0]
print(f"Measured range rate: {doppler_factor.measuredRangeRate():.3f} m/s")
print(f"Line of sight: {doppler_factor.lineOfSight()}")
print(f"Range-rate residual: {residual:.6f} m/s")
assert doppler_factor.dt() == dtDopplerFactorArm¶
A rotating platform moves its antenna relative to the body origin. DopplerFactorArm models that motion as v_antenna = v_body + R_body (omega \times lever_arm), so the pose key contributes the attitude needed to rotate the lever-arm velocity. When omega is zero, the factor reduces to DopplerFactor at the same receiver velocity.
pose = gtsam.Pose3(
gtsam.Rot3.RzRyRx(0.3, -0.2, 0.5), receiver_position
)
lever_arm = np.array([0.5, -0.3, 1.0]) # Antenna offset in body frame [m].
angular_velocity = np.array([0.02, -0.05, 0.1]) # Body rate [rad/s].
arm_factor = gtsam.DopplerFactorArm(
0, 1, 2, 3, -1500.0, LAMBDA_L1,
satellite_position, satellite_velocity, receiver_position,
lever_arm, angular_velocity, dt, satellite_clock_drift, noise_model
)
arm_residual = arm_factor.evaluateError(
pose, receiver_velocity, bias_previous, bias_current
)[0]
print(f"Rotating-antenna residual: {arm_residual:.6f} m/s")
zero_rate_arm = gtsam.DopplerFactorArm(
0, 1, 2, 3, -1500.0, LAMBDA_L1,
satellite_position, satellite_velocity, receiver_position,
lever_arm, np.zeros(3), dt, satellite_clock_drift, noise_model
)
zero_rate_residual = zero_rate_arm.evaluateError(
pose, receiver_velocity, bias_previous, bias_current
)[0]
np.testing.assert_allclose(zero_rate_residual, residual, atol=1e-9)
print("With zero angular velocity, DopplerFactorArm matches DopplerFactor.")Local navigation-frame states¶
Pass ecef_T_nav to use a local navigation-frame pose and velocity while keeping the satellite geometry in ECEF. The angular velocity and lever arm remain expressed in the body frame.
ecef_T_nav = gtsam.Pose3(
gtsam.Rot3.RzRyRx(0.1, 0.4, -0.7), receiver_position
)
nav_pose = gtsam.Pose3(
gtsam.Rot3.RzRyRx(0.3, -0.2, 0.5), np.zeros(3)
)
nav_velocity = np.array([0.3, -0.1, 0.05])
nav_factor = gtsam.DopplerFactorArm(
0, 1, 2, 3, -1500.0, LAMBDA_L1,
satellite_position, satellite_velocity, receiver_position,
lever_arm, ecef_T_nav, angular_velocity, dt,
satellite_clock_drift, noise_model
)
nav_residual = nav_factor.evaluateError(
nav_pose, nav_velocity, bias_previous, bias_current
)[0]
print(f"Navigation-frame residual: {nav_residual:.6f} m/s")Source¶
DopplerVelocityExample
.ipynb - the factors on real 1 Hz GNSS data