Planning and Localization

Relevant source files

• openpilot/common/transformations/orientation.py
• openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
• openpilot/selfdrive/controls/lib/longitudinal_planner.py
• openpilot/selfdrive/controls/plannerd.py
• openpilot/selfdrive/controls/radard.py
• openpilot/selfdrive/locationd/lagd.py
• openpilot/selfdrive/locationd/test/test_lagd.py
• openpilot/selfdrive/test/longitudinal_maneuvers/plant.py
• openpilot/selfdrive/ui/mici/onroad/model_renderer.py
• openpilot/selfdrive/ui/onroad/model_renderer.py
• openpilot/tools/replay/lib/ui_helpers.py
• tools/release/build_stripped.sh

The Planning and Localization stack in openpilot is responsible for determining the vehicle's precise state in the world and calculating the optimal trajectories for both longitudinal (acceleration/braking) and lateral (steering) control. This system bridges the gap between raw perception (vision/radar) and the control actuation layer.

1. Planning System (plannerd)

The plannerd daemon serves as the central orchestration point for high-level driving logic. It primarily manages longitudinal planning and safety features like Lane Departure Warning (LDW).

Longitudinal Planning

The LongitudinalPlanner class calculates the desired acceleration and velocity profile. It uses a Model Predictive Control (MPC) approach to balance multiple objectives: following a lead car, maintaining cruise speed, and adhering to comfort/safety limits.

• Data Sources: It consumes modelV2 (vision-based paths and leads), radarState (physical lead measurements), and carState (current vehicle speed) openpilot/selfdrive/controls/plannerd.py22-23
• MPC Integration: It utilizes LongitudinalMpc to solve for the optimal trajectory over a 10-second horizon openpilot/selfdrive/controls/lib/longitudinal_planner.py51
• Turn Limiting: Acceleration is dynamically limited in turns based on lateral acceleration to prevent aggressive speed increases when the vision model might lose a target openpilot/selfdrive/controls/lib/longitudinal_planner.py34-45
• E2E Support: It supports "Experimental Mode" (End-to-End) by blending vision-model predicted accelerations with classical MPC solutions openpilot/selfdrive/controls/lib/longitudinal_planner.py136-143
• Personality Modes: The planner adjusts follow distance and jerk limits based on the driver's selected personality (Relaxed, Standard, Aggressive) openpilot/selfdrive/controls/lib/longitudinal_planner.py112-114
• Coast Acceleration: Calculates a natural coasting acceleration based on the vehicle's pitch to improve throttle logic openpilot/selfdrive/controls/lib/longitudinal_planner.py31-32

Lane Departure Warning (LDW)

plannerd runs the LaneDepartureWarning logic, which monitors the vehicle's position relative to lane lines and triggers alerts if a departure is imminent without driver intent (e.g., no turn signal) openpilot/selfdrive/controls/plannerd.py31-36

Sources: openpilot/selfdrive/controls/plannerd.py11-41 openpilot/selfdrive/controls/lib/longitudinal_planner.py48-160 openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py25-87

---

2. Longitudinal MPC (long_mpc)

The longitudinal trajectory is optimized using the acados solver, which handles a non-linear least squares problem.

MPC State and Constraints

The system defines a state vector $x = [x_{ego}, v_{ego}, a_{ego}]^T$ and control input $u = [j_{ego}]$ (jerk) openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py94-99

Parameter	Role
T_IDXS	Non-uniform time steps for the prediction horizon (up to 10s) openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py51-53
T_FOLLOW	Desired time gap to the lead vehicle, varying by LongitudinalPersonality openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py73-81
DANGER_ZONE_COST	Penalty for entering the unsafe distance buffer behind a lead openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py41
A_CHANGE_COST	Cost associated with rapid changes in acceleration to ensure smoothness openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py40
J_EGO_COST	Penalty on jerk to maintain passenger comfort openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py39

Logic Flow: Planner to Control

Sources: openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py89-213 openpilot/selfdrive/controls/lib/longitudinal_planner.py130-149

---

3. Localization and Parameter Learning

Localization and vehicle parameter estimation are handled by a fusion of IMU, GPS, and vehicle CAN data across several specialized daemons.

Camera Calibration (calibrationd)

This daemon determines the extrinsic orientation of the camera relative to the vehicle frame (Pitch, Yaw, and Roll).

• Process: It filters camera-reported transforms to build a stable calibration. It initializes with HEIGHT_INIT for camera height openpilot/selfdrive/locationd/calibrationd.py9
• Recalibration: The system uses status checks to determine if the device requires recalibration.
• UI Integration: The ModelRenderer in the UI consumes liveCalibration data to correctly project 3D points (paths, lane lines) into the 2D screen space openpilot/selfdrive/ui/onroad/model_renderer.py100-101

Vehicle Parameter Learning (paramsd)

paramsd learns vehicle-specific physical properties while driving.

• States: It estimates parameters like steering ratio and angle offsets.
• Integration: It provides angleOffsetDeg which is used by the LongitudinalPlanner to calculate steering-angle-without-offset for turn-based acceleration limiting openpilot/selfdrive/controls/lib/longitudinal_planner.py89-90

Torque Estimation (torqued)

torqued estimates parameters required for lateral torque control.

• Learning: It estimates the relationship between torque and lateral acceleration.
• UI Feedback: The ModelRenderer uses torque output from carOutput to visualize steering effort openpilot/selfdrive/ui/mici/onroad/model_renderer.py100

Lateral Lag Estimation (lagd)

lagd estimates the mechanical and electrical delay in the steering actuator.

• Method: It uses masked_normalized_cross_correlation to find the time shift between the planner's desiredCurvature (from controlsState) and the IMU's angularVelocityDevice.z (actual yaw rate from livePose) openpilot/selfdrive/locationd/lagd.py55-109 openpilot/selfdrive/locationd/test/test_lagd.py37-39
• Filtering: Estimates are averaged across blocks via BlockAverage and filtered to provide a stable lateralDelay value openpilot/selfdrive/locationd/lagd.py137-170
• Validation: It requires a minimum speed (MIN_VEGO) and yaw rate to ensure signal quality openpilot/selfdrive/locationd/lagd.py24-25

Pose Estimation (locationd)

locationd fuses IMU data with camera odometry to provide a high-frequency livePose message.

• IMU Data: Processes accelerometer and gyroscope data.
• Radar Matching: radard uses the vehicle speed (vEgo) and vision leads to match radar tracks to model-predicted objects openpilot/selfdrive/controls/radard.py119-140

Sources: openpilot/selfdrive/locationd/lagd.py173-182 openpilot/selfdrive/locationd/test/test_lagd.py8-46 openpilot/selfdrive/controls/radard.py186-196 openpilot/selfdrive/ui/onroad/model_renderer.py84-132

---

4. Entity Mapping: Natural Language to Code

The following table and diagram map system responsibilities to specific code entities.

Concept	Code Entity	File Path
Longitudinal Plan	LongitudinalPlanner	openpilot/selfdrive/controls/lib/longitudinal_planner.py48
MPC Solver	LongitudinalMpc	openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py216
Actuator Delay	LateralLagEstimator	openpilot/selfdrive/locationd/lagd.py173
Radar Processing	RadarD	openpilot/selfdrive/controls/radard.py186
Path Rendering	ModelRenderer	openpilot/selfdrive/ui/onroad/model_renderer.py45

System Data Flow Diagram

Sources: openpilot/selfdrive/controls/plannerd.py21-36 openpilot/selfdrive/locationd/lagd.py173-182 openpilot/selfdrive/controls/radard.py186-196 openpilot/selfdrive/controls/lib/longitudinal_planner.py48-65