feat(lateral): add confidence-gated lane centering

Adapt the lane-centering implementation from PR #74 by @jc01rho.

Keep controls in Galaxy developer settings, add confidence and geometry gates, preserve E2E path authority, and reset all related offsets through Safe Mode.

common/params_keys.h

@@ -371,6 +371,9 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"IssueReported", {CLEAR_ON_MANAGER_START, JSON, "{}", "{}"}},
     {"KonikDongleId", {PERSISTENT, STRING, "", "", 0}},
     {"KonikMinutes", {PERSISTENT, INT, "0", "0", 0}},
+    {"LaneCentering", {PERSISTENT, BOOL, "0", "0", 2}},
+    {"LaneCenteringE2EAuthority", {PERSISTENT, FLOAT, "1.0", "1.0", 3}},
+    {"LaneCenterOffset", {PERSISTENT, FLOAT, "0.0", "0.0", 3}},
     {"LaneChanges", {PERSISTENT, BOOL, "1", "1", 0, SETTINGS_SIMPLE}},
     {"LaneChangeSmoothing", {PERSISTENT, INT, "5", "10", 1, SETTINGS_SIMPLE}},
     {"LaneChangeTime", {PERSISTENT, FLOAT, "1.0", "0.0", 1, SETTINGS_SIMPLE}},

selfdrive/controls/controlsd.py

@@ -14,6 +14,7 @@ from opendbc.car.chrysler.values import pacifica_hybrid_aol_stock_acc_mode
 from opendbc.car.gm.values import CAR as GM_CAR
 from opendbc.car.vehicle_model import VehicleModel
 from openpilot.selfdrive.controls.lib.drive_helpers import MAX_LATERAL_JERK, clip_curvature, get_lateral_active
+from openpilot.selfdrive.controls.lib.lane_centering import LaneCenteringController
 from openpilot.selfdrive.controls.lib.latcontrol import LatControl
 from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
 from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD
@@ -305,6 +306,7 @@ class Controls:
     self.curvature = 0.0
     self.desired_curvature = 0.0
     self.lc_smooth_release = 0.0
+    self.lane_centering = LaneCenteringController()
     self.lc_entry_sign = 0.0
     self.lc_arrest_jerk_factor = 1.0
     self.turn_hold_curvature = 0.0
@@ -419,6 +421,7 @@ class Controls:
 
     if not CC.latActive:
       self.LaC.reset()
+      self.lane_centering.reset()
     if not CC.longActive:
       self.LoC.reset()
 
@@ -585,6 +588,14 @@ class Controls:
             held_mag = min(lead_curvature * blinker_dir, abs(self.turn_hold_curvature) + CURVATURE_HOLD_RATCHET_RATE * DT_CTRL)
             self.turn_hold_curvature = math.copysign(held_mag, lead_curvature)
 
+    new_desired_curvature = self.lane_centering.update(
+      new_desired_curvature, model_v2, CS.vEgo,
+      self.starpilot_toggles.lane_centering,
+      self.starpilot_toggles.lane_center_offset,
+      self.starpilot_toggles.lane_centering_e2e_authority,
+      CC.latActive,
+      bool(self.sm.all_checks(['modelV2'])))
+
     jerk_factor = 1.0
     if self.starpilot_toggles.lane_change_pace < 10:
       set_jerk = self.starpilot_toggles.lane_change_jerk_factor

selfdrive/controls/lib/lane_centering.py

@@ -0,0 +1,134 @@
+from cereal import log
+import numpy as np
+
+from openpilot.common.realtime import DT_CTRL
+from openpilot.selfdrive.controls.lib.drive_helpers import smooth_value
+
+
+_MIN_V_EGO = 5.0
+_MIN_LANE_PROB = 0.6
+_MAX_LANE_STD = 0.3
+_MIN_LANE_WIDTH = 2.6
+_MAX_LANE_WIDTH = 4.8
+_MAX_OFFSET = 0.3
+_MIN_CENTER_TO_LINE = 1.1
+_MAX_RAW_CORRECTION = 0.004
+_MAX_GAIN = 0.30
+_SMOOTH_TAU = 0.4
+
+_E2E_MAX_PATH_STD = 0.35
+_E2E_BREAK_IN_START = 0.25
+_E2E_BREAK_IN_FULL = 0.75
+
+
+class LaneCenteringController:
+  def __init__(self) -> None:
+    self._correction = 0.0
+
+  def reset(self) -> None:
+    self._correction = 0.0
+
+  def update(self, model_curvature, model_v2, v_ego, enabled, offset, e2e_authority, lat_active, model_valid) -> float:
+    model_curvature = float(model_curvature)
+
+    try:
+      v_ego = float(v_ego)
+      offset = float(offset)
+      e2e_authority = float(e2e_authority)
+    except (TypeError, ValueError):
+      self.reset()
+      return model_curvature
+
+    if not np.isfinite([v_ego, offset, e2e_authority]).all():
+      self.reset()
+      return model_curvature
+
+    if not model_valid or not enabled or not lat_active or v_ego < _MIN_V_EGO:
+      self.reset()
+      return model_curvature
+
+    try:
+      if model_v2.meta.laneChangeState != log.LaneChangeState.off:
+        self.reset()
+        return model_curvature
+    except (AttributeError, TypeError, ValueError):
+      self.reset()
+      return model_curvature
+
+    valid, raw_correction = self._raw_correction(
+      model_v2,
+      v_ego,
+      float(np.clip(offset, -_MAX_OFFSET, _MAX_OFFSET)),
+      float(np.clip(e2e_authority, 0.0, 1.0)),
+    )
+    if not valid:
+      self.reset()
+      return model_curvature
+
+    target = float(np.clip(raw_correction, -_MAX_RAW_CORRECTION, _MAX_RAW_CORRECTION)) * _MAX_GAIN
+    self._correction = float(smooth_value(target, self._correction, _SMOOTH_TAU, dt=DT_CTRL))
+    return model_curvature + self._correction
+
+  @staticmethod
+  def _valid_path(x, y) -> bool:
+    return x.size >= 2 and x.size == y.size and np.isfinite(x).all() and np.isfinite(y).all() and np.all(np.diff(x) > 0)
+
+  @staticmethod
+  def _covers(x, distance: float) -> bool:
+    return bool(x[0] <= distance <= x[-1])
+
+  def _raw_correction(self, model_v2, v_ego: float, offset: float, e2e_authority: float) -> tuple[bool, float]:
+    try:
+      lane_lines = model_v2.laneLines
+      probs = np.asarray(model_v2.laneLineProbs, dtype=float)
+      stds = np.asarray(model_v2.laneLineStds, dtype=float)
+      if len(lane_lines) < 3 or probs.size < 3 or stds.size < 3:
+        return False, 0.0
+      if not np.isfinite(probs[[1, 2]]).all() or not np.isfinite(stds[[1, 2]]).all():
+        return False, 0.0
+      if np.any(probs[[1, 2]] < _MIN_LANE_PROB) or np.any(probs[[1, 2]] > 1.0):
+        return False, 0.0
+      if np.any(stds[[1, 2]] < 0.0) or np.any(stds[[1, 2]] > _MAX_LANE_STD):
+        return False, 0.0
+
+      left_x = np.asarray(lane_lines[1].x, dtype=float)
+      left_y = np.asarray(lane_lines[1].y, dtype=float)
+      right_x = np.asarray(lane_lines[2].x, dtype=float)
+      right_y = np.asarray(lane_lines[2].y, dtype=float)
+      pos_x = np.asarray(model_v2.position.x, dtype=float)
+      pos_y = np.asarray(model_v2.position.y, dtype=float)
+      if not (self._valid_path(left_x, left_y) and self._valid_path(right_x, right_y) and self._valid_path(pos_x, pos_y)):
+        return False, 0.0
+
+      lookahead = float(np.clip(v_ego, 8.0, 35.0))
+      if not all(self._covers(x, lookahead) for x in (left_x, right_x, pos_x)):
+        return False, 0.0
+
+      left = float(np.interp(lookahead, left_x, left_y))
+      right = float(np.interp(lookahead, right_x, right_y))
+      width = right - left
+      if not _MIN_LANE_WIDTH <= width <= _MAX_LANE_WIDTH:
+        return False, 0.0
+
+      max_safe_offset = min(_MAX_OFFSET, max(0.0, width * 0.5 - _MIN_CENTER_TO_LINE))
+      target_y = 0.5 * (left + right) + float(np.clip(offset, -max_safe_offset, max_safe_offset))
+      model_y = float(np.interp(lookahead, pos_x, pos_y))
+      error = target_y - model_y
+
+      try:
+        pos_y_std = np.asarray(model_v2.position.yStd, dtype=float)
+        if self._valid_path(pos_x, pos_y_std):
+          path_std = float(np.interp(lookahead, pos_x, pos_y_std))
+          if 0.0 <= path_std <= _E2E_MAX_PATH_STD:
+            break_in = np.clip(
+              (abs(error) - _E2E_BREAK_IN_START) / (_E2E_BREAK_IN_FULL - _E2E_BREAK_IN_START),
+              0.0,
+              1.0,
+            )
+            error *= 1.0 - e2e_authority * float(break_in)
+      except (AttributeError, TypeError, ValueError):
+        pass
+
+      return True, float(2.0 * error / lookahead ** 2)
+    except (AttributeError, IndexError, TypeError, ValueError):
+      return False, 0.0

selfdrive/controls/tests/test_lane_centering.py

@@ -0,0 +1,138 @@
+from types import SimpleNamespace
+
+import numpy as np
+import pytest
+
+from openpilot.selfdrive.controls.lib.lane_centering import LaneCenteringController
+
+
+_V_EGO = 20.0
+_XS = np.linspace(0.0, 50.0, 52)
+
+
+def _path(y, y_std=0.1):
+  return SimpleNamespace(
+    x=_XS.copy(),
+    y=np.full_like(_XS, float(y)),
+    yStd=np.full_like(_XS, float(y_std)),
+  )
+
+
+def _model(left=-1.8, right=1.8, model_y=0.0, lane_prob=0.9, lane_std=0.1, path_std=0.1, lane_change=0):
+  return SimpleNamespace(
+    laneLines=[_path(0.0), _path(left), _path(right), _path(0.0)],
+    laneLineProbs=[0.0, lane_prob, lane_prob, 0.0],
+    laneLineStds=[0.0, lane_std, lane_std, 0.0],
+    position=_path(model_y, path_std),
+    meta=SimpleNamespace(laneChangeState=lane_change),
+  )
+
+
+def _update(controller, model, *, offset=0.0, authority=1.0, enabled=True, active=True, valid=True, speed=_V_EGO):
+  return controller.update(0.0, model, speed, enabled, offset, authority, active, valid)
+
+
+def _converge(model, *, offset=0.0, authority=1.0):
+  controller = LaneCenteringController()
+  output = 0.0
+  for _ in range(300):
+    output = _update(controller, model, offset=offset, authority=authority)
+  return controller, output
+
+
+@pytest.mark.parametrize(
+  "kwargs",
+  [
+    {"enabled": False},
+    {"active": False},
+    {"valid": False},
+    {"speed": 4.9},
+  ],
+)
+def test_hard_gates_are_noop(kwargs):
+  assert _update(LaneCenteringController(), _model(left=-1.5, right=2.1), **kwargs) == 0.0
+
+
+def test_lane_change_is_noop():
+  assert _update(LaneCenteringController(), _model(left=-1.5, right=2.1, lane_change=1)) == 0.0
+
+
+@pytest.mark.parametrize(
+  "field,value",
+  [
+    ("prob", np.nan),
+    ("prob", 1.1),
+    ("std", np.nan),
+    ("std", -0.1),
+  ],
+)
+def test_invalid_lane_confidence_is_rejected(field, value):
+  model = _model(left=-1.5, right=2.1)
+  values = model.laneLineProbs if field == "prob" else model.laneLineStds
+  values[1] = value
+  assert _update(LaneCenteringController(), model) == 0.0
+
+
+def test_input_must_cover_lookahead():
+  model = _model(left=-1.5, right=2.1)
+  model.laneLines[1].x = model.laneLines[1].x[:10]
+  model.laneLines[1].y = model.laneLines[1].y[:10]
+  assert _update(LaneCenteringController(), model) == 0.0
+
+
+def test_lane_center_error_steers_toward_center():
+  _, right = _converge(_model(left=-1.5, right=2.1), authority=0.0)
+  _, left = _converge(_model(left=-2.1, right=1.5), authority=0.0)
+  assert right > 0.0
+  assert left < 0.0
+
+
+def test_offset_direction():
+  _, right = _converge(_model(), offset=0.2, authority=0.0)
+  _, left = _converge(_model(), offset=-0.2, authority=0.0)
+  assert right > 0.0
+  assert left < 0.0
+
+
+def test_offset_is_reduced_in_narrow_lane():
+  narrow = _model(left=-1.3, right=1.3)
+  _, at_safe_limit = _converge(narrow, offset=0.2, authority=0.0)
+  _, above_safe_limit = _converge(narrow, offset=0.3, authority=0.0)
+  assert np.isclose(at_safe_limit, above_safe_limit)
+
+
+def test_confident_e2e_path_can_fully_break_in():
+  model = _model(left=-1.0, right=2.6, model_y=0.0, path_std=0.1)
+  _, lane_authority = _converge(model, authority=0.0)
+  _, e2e_authority = _converge(model, authority=1.0)
+  assert lane_authority > 0.0
+  assert abs(e2e_authority) < 1e-9
+
+
+def test_uncertain_e2e_path_does_not_break_in():
+  model = _model(left=-1.0, right=2.6, model_y=0.0, path_std=0.6)
+  _, output = _converge(model, authority=1.0)
+  assert output > 0.0
+
+
+def test_e2e_authority_blends_lane_correction():
+  model = _model(left=-1.2, right=2.4, model_y=0.0, path_std=0.1)
+  _, lane_only = _converge(model, authority=0.0)
+  _, blended = _converge(model, authority=0.5)
+  _, e2e = _converge(model, authority=1.0)
+  assert lane_only > blended > e2e >= 0.0
+
+
+def test_confidence_loss_drops_filtered_correction():
+  controller, output = _converge(_model(left=-1.5, right=2.1), authority=0.0)
+  assert output > 0.0
+  assert _update(controller, _model(left=-1.5, right=2.1, lane_prob=0.2), authority=0.0) == 0.0
+
+
+def test_correction_is_smoothed_and_capped():
+  controller = LaneCenteringController()
+  model = _model(left=0.0, right=3.0, path_std=0.6)
+  first = _update(controller, model, authority=0.0)
+  _, steady = _converge(model, authority=0.0)
+  assert 0.0 < first < steady
+  assert np.isclose(steady, 0.004 * 0.30, atol=1e-6)

starpilot/common/safe_mode.py

@@ -33,6 +33,10 @@ SAFE_MODE_MANAGED_KEYS = (
   "SteerKP",
   "SteerLatAccel",
   "SteerRatio",
+  "CameraOffset",
+  "LaneCentering",
+  "LaneCenteringE2EAuthority",
+  "LaneCenterOffset",
   "LaneChanges",
   "LaneChangeTime",
   "LaneDetectionWidth",

starpilot/common/starpilot_variables.py

@@ -705,6 +705,7 @@ class StarPilotVariables:
     toggle.use_wheel_speed = self.get_value("WheelSpeed", condition=advanced_custom_ui)
 
     advanced_lateral_tuning = self.get_value("AdvancedLateralTune")
+    toggle.lane_centering = self.get_value("LaneCentering")
     toggle.force_auto_tune = self.get_value("ForceAutoTune", condition=advanced_lateral_tuning and not has_auto_tune and is_torque_car and not is_angle_car)
     # Force-off is also meaningful on manually tuned torque cars: it locks the
     # vehicle-model parameters instead of allowing paramsd to learn over them.
@@ -729,6 +730,11 @@ class StarPilotVariables:
     honda_pid_lateral = toggle.car_make == "honda" and CP.lateralTuning.which() == "pid" and not is_angle_car
     toggle.honda_lateral_pid_kp_scale = self.get_value("HondaLateralPidKpScale", cast=float, condition=honda_pid_lateral, default=1.0, min=0.1, max=4.0)
     toggle.honda_lateral_pid_ki_scale = self.get_value("HondaLateralPidKiScale", cast=float, condition=honda_pid_lateral, default=1.0, min=0.1, max=4.0)
+    toggle.lane_center_offset = self.get_value("LaneCenterOffset", cast=float, condition=toggle.lane_centering, default=0.0, min=-0.3, max=0.3)
+    toggle.lane_centering_e2e_authority = self.get_value(
+      "LaneCenteringE2EAuthority", cast=float, condition=toggle.lane_centering,
+      default=1.0, min=0.0, max=1.0,
+    )
 
     advanced_longitudinal_tuning = toggle.openpilot_longitudinal and self.get_value("AdvancedLongitudinalTune")
     ev_vehicle = default_ev_tuning_enabled(CP)

starpilot/common/tests/test_lane_centering_galaxy.py

@@ -0,0 +1,46 @@
+import json
+from pathlib import Path
+
+
+_LAYOUT_PATH = Path(__file__).resolve().parents[2] / "system/the_galaxy/assets/components/tools/device_settings_layout.json"
+
+
+def _sections():
+  layout = json.loads(_LAYOUT_PATH.read_text(encoding="utf-8"))
+  return {
+    section["name"]: {param["key"]: param for param in section["params"]}
+    for section in layout
+  }
+
+
+def test_lane_centering_is_only_in_galaxy_developer_section():
+  sections = _sections()
+  keys = {"LaneCentering", "LaneCenterOffset", "LaneCenteringE2EAuthority"}
+
+  assert keys <= sections["Developer"].keys()
+  for name, params in sections.items():
+    if name != "Developer":
+      assert keys.isdisjoint(params)
+
+  for key in keys:
+    assert sections["Developer"][key]["settings_tier"] == "advanced"
+
+
+def test_lane_centering_galaxy_controls():
+  developer = _sections()["Developer"]
+  centering = developer["LaneCentering"]
+  offset = developer["LaneCenterOffset"]
+  e2e_authority = developer["LaneCenteringE2EAuthority"]
+
+  assert centering["ui_type"] == "toggle"
+  assert centering["is_parent_toggle"] is True
+
+  assert offset["parent_key"] == "LaneCentering"
+  assert offset["min"] == -0.3
+  assert offset["max"] == 0.3
+  assert offset["step"] == 0.01
+
+  assert e2e_authority["parent_key"] == "LaneCentering"
+  assert e2e_authority["min"] == 0.0
+  assert e2e_authority["max"] == 1.0
+  assert e2e_authority["step"] == 0.05

starpilot/common/tests/test_safe_mode_lane_centering.py

@@ -0,0 +1,10 @@
+from openpilot.starpilot.common.safe_mode import SAFE_MODE_MANAGED_KEYS
+
+
+def test_safe_mode_manages_lane_centering_settings():
+  assert {
+    "CameraOffset",
+    "LaneCentering",
+    "LaneCenteringE2EAuthority",
+    "LaneCenterOffset",
+  } <= set(SAFE_MODE_MANAGED_KEYS)

starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json

@@ -3972,6 +3972,41 @@
         "precision": 2,
         "settings_tier": "advanced"
       },
+      {
+        "key": "LaneCentering",
+        "label": "Lane Centering",
+        "description": "Experimentally bias the model command toward the detected lane center. Requires two confident lane lines and remains subject to normal curvature and jerk limits.",
+        "data_type": "bool",
+        "ui_type": "toggle",
+        "is_parent_toggle": true,
+        "settings_tier": "advanced"
+      },
+      {
+        "key": "LaneCenterOffset",
+        "label": "Lane Center Offset",
+        "description": "Shift the lane target left or right in meters. The controller reduces this offset automatically when the detected lane is narrow.",
+        "data_type": "float",
+        "ui_type": "numeric",
+        "min": -0.3,
+        "max": 0.3,
+        "step": 0.01,
+        "precision": 2,
+        "parent_key": "LaneCentering",
+        "settings_tier": "advanced"
+      },
+      {
+        "key": "LaneCenteringE2EAuthority",
+        "label": "E2E Break-In Authority",
+        "description": "How strongly a confident end-to-end path can override lane centering when it deliberately departs the lane target. 1.0 gives the model full authority; 0.0 disables break-in.",
+        "data_type": "float",
+        "ui_type": "numeric",
+        "min": 0.0,
+        "max": 1.0,
+        "step": 0.05,
+        "precision": 2,
+        "parent_key": "LaneCentering",
+        "settings_tier": "advanced"
+      },
       {
         "key": "HondaLateralPidKpScale",
         "label": "Honda PID Kp Scale",

starpilot/system/the_galaxy/the_galaxy.py

@@ -959,6 +959,10 @@ _TROUBLESHOOT_ADVANCED_LATERAL_KEYS = [
   "ForceAutoTune",
   "ForceAutoTuneOff",
   "ForceTorqueController",
+  "CameraOffset",
+  "LaneCentering",
+  "LaneCenteringE2EAuthority",
+  "LaneCenterOffset",
 ]
 
 _TROUBLESHOOT_ADVANCED_LONGITUDINAL_KEYS = [