ConstantVelocityFactor

Overview¶

The ConstantVelocityFactor (contributed by Asa Hammond in 2021) is a simple motion model factor that connects two NavState variables at different times, $t_i$ and $t_j$ . It enforces the assumption that the velocity remained constant between the two time steps.

Given NavState $X_i$ (containing pose $T_i$ and velocity $v_i$ ) and NavState $X_j$ (containing pose $T_j$ and velocity $v_j$ ), and the time difference $\Delta t = t_j - t_i$ , this factor penalizes deviations from the constant velocity prediction.

Mathematical Formulation¶

The factor uses the NavState::update method (or equivalent logic internally) to predict the state at time $t_j$ based on the state at $t_i$ and the assumption of constant velocity (and zero acceleration/angular velocity). Let this prediction be $X_{j, pred}$ :

X_{j, pred} = X_i . \text{update}(\text{accel}=0, \text{omega}=0, \Delta t)

(1)

Essentially, this integrates the velocity $v_i$ over $\Delta t$ to predict the change in position, while keeping orientation and velocity constant:

R_{j, pred} = R_i

(2)

v_{j, pred} = v_i

(3)

p_{j, pred} = p_i + R_i (v_i^{body}) \Delta t \quad \text{(using body velocity update)}

(4)

The factor’s 9-dimensional error $e$ is the difference between the predicted state $X_{j, pred}$ and the actual state $X_j$ , expressed in the tangent space at $X_{j,pred}$ :

e = \text{localCoordinates}_{X_{j, pred}}(X_j)

(5)

The noise model associated with the factor determines how strongly deviations from this constant velocity prediction are penalized.

Key Functionality / API¶

Constructor: ConstantVelocityFactor(key1, key2, dt, model): Creates the factor connecting the NavState at key1 (time $i$ ) and key2 (time $j$ ), given the time difference dt and a 9D noise model.
evaluateError(state1, state2): Calculates the 9D error vector based on the constant velocity prediction from state1 to state2 over the stored dt.

Usage Example¶

This factor is often used as a simple process model between consecutive states when higher-fidelity IMU integration is not available or needed.

import gtsam
import numpy as np
from gtsam.symbol_shorthand import X # Using X for NavState keys here

# Define keys for two NavStates
key1 = X(0)
key2 = X(1)

# Time difference between states
dt = 0.1 # seconds

# Define a noise model - how much deviation from constant velocity is allowed?
# Example: Tighter on rotation/velocity, looser on position change due to velocity
rot_sigma = 0.01 # rad
pos_sigma = 0.1 # m 
vel_sigma = 0.05 # m/s
sigmas = np.concatenate([
    np.full(3, rot_sigma), 
    np.full(3, pos_sigma),
    np.full(3, vel_sigma)
])
noise_model = gtsam.noiseModel.Diagonal.Sigmas(sigmas)

# Create the factor
cv_factor = gtsam.ConstantVelocityFactor(key1, key2, dt, noise_model)

print("Created ConstantVelocityFactor:")
cv_factor.print()

# --- Example Evaluation ---
# State 1: At origin, moving along +X at 1 m/s
pose1 = gtsam.Pose3()
vel1 = np.array([1.0, 0.0, 0.0])
state1 = gtsam.NavState(pose1, vel1)

# State 2: Exactly matches constant velocity prediction
pose2 = gtsam.Pose3(gtsam.Rot3(), np.array([1.0*dt, 0.0, 0.0]))
vel2 = vel1 # Constant velocity
state2_perfect = gtsam.NavState(pose2, vel2)

error_perfect = cv_factor.evaluateError(state1, state2_perfect)
print("\nError for perfect prediction (should be zero):", error_perfect)

# State 3: Velocity changed slightly
vel3 = np.array([1.0, 0.1, 0.0]) # Added Y velocity
state3_vel_change = gtsam.NavState(pose2, vel3) # Keep predicted pose

error_vel_change = cv_factor.evaluateError(state1, state3_vel_change)
print("Error for velocity change:", error_vel_change)

# State 4: Position is slightly off prediction
pose4 = gtsam.Pose3(gtsam.Rot3(), np.array([1.0*dt, 0.05, 0.0])) # Y pos is off
state4_pos_change = gtsam.NavState(pose4, vel2) # Keep velocity

error_pos_change = cv_factor.evaluateError(state1, state4_pos_change)
print("Error for position change:", error_pos_change)

Created ConstantVelocityFactor:
  keys = { x0 x1 }
  noise model: diagonal sigmas [0.01; 0.01; 0.01; 0.1; 0.1; 0.1; 0.05; 0.05; 0.05];

Error for perfect prediction (should be zero): [0. 0. 0. 0. 0. 0. 0. 0. 0.]
Error for velocity change: [0.  0.  0.  0.  0.  0.  0.  0.1 0. ]
Error for position change: [0.   0.   0.   0.   0.05 0.   0.   0.   0.  ]

Source¶

ConstantVelocityFactor.h