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

> Learn how the locationd Kalman filter in openpilot fuses IMU and visual odometry data to accurately estimate vehicle pose and achieve real-time localization.

- Repository: [comma.ai/openpilot](https://github.com/commaai/openpilot)
- Tags: internals
- Published: 2026-03-05

---

**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`](https://github.com/commaai/openpilot/blob/main/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:

```python
self.kf = PoseKalman(GENERATED_DIR, MAX_FILTER_REWIND_TIME)

```

`GENERATED_DIR` is defined in [`selfdrive/locationd/models/constants.py`](https://github.com/commaai/openpilot/blob/main/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:

```python
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`](https://github.com/commaai/openpilot/blob/main/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

```python
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

```python

# 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

```python
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`](https://github.com/commaai/openpilot/blob/main/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.