How the locationd Kalman Filter Estimates Vehicle Pose and Localization in Openpilot

The locationd Kalman filter runs an Extended Kalman Filter (EKF) called PoseKalman that fuses high-rate IMU data with visual odometry to maintain an 18-state estimate of vehicle orientation, velocity, and sensor biases in the NED frame.

The locationd service in the commaai/openpilot repository provides real-time vehicle localization for autonomous driving. By implementing the locationd Kalman filter as a tightly-coupled sensor fusion system, it tracks the vehicle's pose using an 18-state EKF that processes accelerometer, gyroscope, and camera odometry inputs to output the livePose message consumed by planning and control modules.

PoseKalman EKF Architecture

State Vector and Process Model

The core estimation engine is defined in selfdrive/locationd/models/pose_kf.py. The States class (lines 21‑27) defines an 18‑element state vector comprising orientation, velocity, angular velocity, gyro bias, linear acceleration, and accelerometer bias.

The process model assumes constant acceleration and angular velocity over small time steps Δt. The symbolic code generator builds the state‑transition matrix f_sym (lines 78‑88) to propagate state estimates between asynchronous sensor updates, compiling into a fast C‑extension for runtime execution.

Observation Models

Four observation kinds correct the state prediction using sensor data:

• PHONE_GYRO (kind 4): Measures angular_velocity + gyro_bias (line 91)
• PHONE_ACCEL (kind 10): Measures device_from_ned·gravity + acceleration + centripetal + acc_bias (line 92)
• CAMERA_ODO_ROTATION (kind 14): Raw angular velocity from visual odometry
• CAMERA_ODO_TRANSLATION (kind 13): Raw linear velocity from visual odometry

These definitions are collected into obs_eqs (lines 95‑100) and fed to the code generator that produces the runtime observation functions.

Filter Lifecycle in locationd

Initialization and Sensor Input Handling

LocationEstimator.__init__ instantiates the filter with generated code paths:

self.kf = PoseKalman(GENERATED_DIR, MAX_FILTER_REWIND_TIME)

GENERATED_DIR is defined in selfdrive/locationd/models/constants.py (line 3), and MAX_FILTER_REWIND_TIME is set to 0.8 seconds to limit out‑of‑order observation acceptance.

Incoming messages route through LocationEstimator.handle_log. For accelerometer data, the raw vector is re‑oriented to match the device frame convention (meas = np.array([-v[2], -v[1], -v[0]])) and fed to the EKF:

acc_res = self.kf.predict_and_observe(sensor_time,
                                      ObservationKind.PHONE_ACCEL,
                                      meas)

For camera odometry, rotation and translation are first transformed by the device‑to‑calibration matrix, their standard deviations are rotated using rotate_std from helpers.py, and then passed to the filter using ObservationKind.CAMERA_ODO_ROTATION and ObservationKind.CAMERA_ODO_TRANSLATION.

Sanity Checks and Data Validation

Before fusing any measurement, locationd performs rigorous validation in handle_log (lines 102‑136):

• Timestamp validation: Sensor time must be within MAX_SENSOR_TIME_DIFF (0.1 s) of filter time
• Magnitude sanity: Acceleration must be < 100 m/s² (ACCEL_SANITY_CHECK) to reject impacts or sensor faults
• Bias cross‑check: Gyro yaw‑rate is compared against the camera odometry yaw‑rate distribution to detect IMU saturation or camera tracking loss

These checks prevent corrupted or delayed data from destabilizing the state estimate.

State Extraction and Publishing

LocationEstimator.get_msg reads the current filter state (self.kf.x) and covariance (self.kf.P), computes per‑state standard deviations via np.sqrt(np.diag(cov)), and packs them into the livePose message. This message contains:

• orientationNED: Roll, pitch, yaw in radians
• velocityDevice: Velocity in the device frame (m/s)
• angularVelocityDevice: Corrected angular rates (rad/s)
• accelerationDevice: Specific force after bias removal (m/s²)

Numerical Stability and Reset Logic

The _finite_check routine (lines 91‑96) monitors the state vector and covariance for non‑finite values (NaN or infinity). If detected, the filter resets to the default prior (PoseKalman.initial_x and PoseKalman.initial_P), ensuring that numerical divergence does not propagate to downstream consumers.

How Localization Is Achieved

Complementary Sensor Fusion

The phone's IMU provides high‑rate (typically 100 Hz) angular rates and specific force, which integrate accurately over short intervals but drift over time. Visual odometry supplies absolute translation and rotation estimates at camera frame rate (typically 20 Hz), correcting accumulated drift while the IMU provides high‑bandwidth motion information between visual updates.

Online Bias Estimation

The state vector includes explicit gyro bias (States.GYRO_BIAS) and accelerometer bias (States.ACCEL_BIAS) terms. By treating biases as part of the state that evolves slowly according to the process model, the EKF continuously learns and subtracts systematic sensor errors. This eliminates the need for offline calibration and maintains accuracy during long drives.

Gravity and Centripetal Compensation

The accelerometer observation model (h_acc_sym) correctly interprets raw readings as specific force by:

1. Transforming gravity into the device frame: device_from_ned * gravity
2. Adding centripetal acceleration: angular_velocity.cross(velocity)

This physics‑based model allows the filter to separate gravitational acceleration from vehicle motion, enabling accurate estimation of road pitch and roll angles.

Covariance Tuning and Robustness

Process noise Q (lines 48‑53) and observation noise obs_noise (lines 55‑58) are carefully tuned for each sensor type. To account for temporal correlation in visual odometry, standard deviations are artificially inflated (×10 for rotation, ×2 for translation), preventing the filter from becoming over‑confident in camera measurements during rapid motion or image blur.

Practical Implementation Examples

Creating the Estimator and Feeding Sensor Messages

from openpilot.selfdrive.locationd.locationd import LocationEstimator, HandleLogResult
from openpilot.selfdrive.locationd.models.constants import ObservationKind

est = LocationEstimator(debug=False)

# Handle an accelerometer message (structure simplified)

t = 1.234567  # sensor timestamp in seconds

acc_msg = {"acceleration": {"v": [0.0, 0.0, -9.81]}}
res = est.handle_log(t, "accelerometer", acc_msg)
assert res == HandleLogResult.SUCCESS

Obtaining the livePose for Publishing

# After processing sensor data

msg = est.get_msg(sensors_valid=True,
                  inputs_valid=True,
                  filter_valid=True)

# Access estimates:

# msg.livePose.orientationNED  - [roll, pitch, yaw] in radians

# msg.livePose.velocityDevice  - 3-axis velocity (m/s)

# msg.livePose.angularVelocityDevice - bias-corrected rates (rad/s)

Resetting the Filter After Numerical Issues

from openpilot.selfdrive.locationd.models.pose_kf import PoseKalman

# Reset to default prior if divergence detected

est.reset(t=0.0,
          x_initial=PoseKalman.initial_x,
          P_initial=PoseKalman.initial_P)

Summary

• The locationd Kalman filter implements an 18‑state EKF (PoseKalman) defined in selfdrive/locationd/models/pose_kf.py to track vehicle pose in the NED frame
• It fuses IMU data (gyroscope and accelerometer) with visual odometry through four specific observation models (kinds 4, 10, 13, 14)
• Online bias estimation continuously corrects for IMU drift without requiring external calibration
• Physics-based observation models account for gravity transformation and centripetal acceleration to accurately interpret accelerometer data
• Multi-layer validation (timestamp checks, magnitude limits, cross‑sensor consistency) rejects outliers before fusion
• Automatic reset logic (_finite_check) ensures numerical stability by reinitializing the filter if divergence occurs

Frequently Asked Questions

What coordinate frame does locationd use for pose estimation?

The filter operates in the NED (North‑East‑Down) frame. Orientation is stored as a rotation from NED to the device frame, while velocity is maintained in the device frame for direct consumption by control algorithms. This convention aligns the Z‑axis with gravity and the X‑axis with geographic north.

How does locationd handle IMU drift during long drives?

The EKF treats gyro and accelerometer biases as part of the 18‑element state vector. These bias states evolve slowly according to the process model, allowing the filter to learn and subtract systematic drift in real‑time using visual odometry as a low‑drift reference. This eliminates the need for periodic recalibration stops.

Why does the filter reset itself during operation?

If the _finite_check routine detects non‑finite values (NaN or infinity) in the state vector or covariance matrix—typically caused by numerical overflow or sensor saturation—the filter automatically resets to the default prior defined in PoseKalman.initial_x and PoseKalman.initial_P. This safety mechanism prevents corrupted estimates from propagating to planning and control modules.

How are camera odometry measurements validated before fusion?

Before accepting visual odometry, locationd checks that timestamps are within 0.1 seconds of the current filter time, rotates standard deviations into the correct frame using rotate_std, and validates that gyro-derived yaw rates are statistically consistent with camera measurements. Outliers that fail these checks are rejected to maintain filter stability.

{
  "mcpServers": {
    "instagit": {
      "command": "npx",
      "args": ["-y", "instagit@latest"]
    }
  }
}