modeld_v2: learned per-model turn correction toward the FSD/driver line, behind the turn switch

A small correction on top of the model's action in slow turns:
  command = action + s(v) * (theta_path * (path - action) + theta_time * (time - action))
with time the plan's curvature at action_t on its time axis, path the plan's
heading at the arc length the car itself covers in action_t, s(v) full below
v_full and gone by v_end, and the command kept within the range action and path
span (+10%), so it only moves the action toward the model's own line. The
weights are fitted per model (openpilot.distill supervised/fit_turn_correction.py)
on frames where the driver or FSD steered, or openpilot took a whole turn with
no push-back. For a model that has one it replaces the 0.75 floor and the path
cap; other models keep those.

Light Speed 2, from routes 98, 9a, 9e, 9f, b0 (26 turns), held out by route:
mean |command - driven| at 1-3 m/s 0.039 -> 0.021 (today's floor 0.029),
3-4.5 m/s 0.028 -> 0.014, 4.5-6 m/s 0.017 -> 0.008, 6-9 m/s 0.006 -> 0.004;
peak command / peak driven per turn 0.74 (floor 0.83) -> 1.02 [0.84-1.26].
Fitted 0.871 of the way to the time-axis curvature (0.055 to the line), full
below 6 m/s, fading out by 10. Not driven yet.

The switch (the TurnInHold key) is now "turn correction".

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>

openpilot/selfdrive/ui/sunnypilot/mici/layouts/models.py

@@ -125,9 +125,10 @@ class ModelsLayoutMici(NavScroller):
                                                            "while the model still sets the cruising speed. Turns off the accel boost. " +
                                                            "Needs openpilot longitudinal. Not available with the Default model."))
 
-    self.path_steering_toggle = BigParamControl(tr("slow-turn path steering (alpha)"), "TurnInHold",
-      description=tr("Below about 15 mph, limits steering to the model's planned line at your actual speed instead of the speed the " +
-                     "model expects, so a slow turn is not taken tighter than planned. Only ever steers less. Fades out by 15 mph. " +
+    self.path_steering_toggle = BigParamControl(tr("turn correction (alpha)"), "TurnInHold",
+      description=tr("In slow turns, corrects the model's steering toward the line FSD and you take, with a correction learned for " +
+                     "that model from your drives. It can only move steering within the range the model's own plan allows. A model " +
+                     "without a learned correction is only kept from turning tighter than its planned line at your speed. " +
                      "Not available with the Default model."))
 
     self.map_turn_toggle = BigParamControl(tr("signal navigation (experimental)"), "MapboxNextTurnEnabled",

openpilot/sunnypilot/modeld_v2/modeld.py

@@ -53,6 +53,7 @@ from openpilot.sunnypilot.modeld_v2.steering_smoothing import lat_smooth_seconds
 from openpilot.sunnypilot.modeld_v2.turn_desire_refresh import TurnDesireRefresh, desire_input
 from openpilot.sunnypilot.modeld_v2.turn_plan_floor import apply_turn_plan_floor
 from openpilot.sunnypilot.modeld_v2 import path_steering
+from openpilot.sunnypilot.modeld_v2.turn_correction import apply_turn_correction, correction_for
 from openpilot.sunnypilot.modeld_v2.plan_long import PlanLong
 from openpilot.sunnypilot.modeld_v2.navigation_input import (NavigationInputBuffer, NavigationStatePublisher, SignalRouteGuard,
                                                           destination_route_context, navigation_capability)
@@ -106,6 +107,7 @@ class ModelState(ModelStateBase):
       model_bundle = get_active_bundle(chestnut=chestnut)
     self.generation = model_bundle.generation if model_bundle is not None else None
     self.model_name = model_bundle.displayName if model_bundle is not None else ''
+    self.turn_correction = correction_for(model_bundle.ref) if model_bundle is not None else None
     overrides = {override.key: override.value for override in model_bundle.overrides} if model_bundle else {}
 
     self.LAT_SMOOTH_SECONDS = float(overrides.get('lat', ".0"))
@@ -267,18 +269,26 @@ class ModelState(ModelStateBase):
                                          action_accel=desired_accel)
       desired_curvature = model_output['action'][0, 0] / (max(1.0, v_ego))**2
       self.action_curvature, self.turn_plan_curvature = desired_curvature, 0.
-      if turn_sign:
-        # the action head under-commits in slow turns while the model's own plan has the turn; use the plan as a floor
-        plan = model_output['plan'][0]
+      plan = model_output['plan'][0]
+      if path_steering_enabled and self.turn_correction is not None:
+        # this model has a learned turn correction toward the FSD/driver line; it replaces the floor and the path cap
         self.turn_plan_curvature = get_curvature_from_plan(plan[:, Plan.T_FROM_CURRENT_EULER][:, 2], plan[:, Plan.ORIENTATION_RATE][:, 2],
                                                            self.constants.T_IDXS, v_ego, lat_action_t)
-        desired_curvature = apply_turn_plan_floor(desired_curvature, self.turn_plan_curvature, turn_sign)
-      if path_steering_enabled and path_steering.path_weight(v_ego) > 0.:
-        plan = model_output['plan'][0]
         path = path_steering.path_curvature(plan[:, Plan.POSITION][:, 0], plan[:, Plan.POSITION][:, 1],
                                             plan[:, Plan.T_FROM_CURRENT_EULER][:, 2], plan[0, Plan.ORIENTATION_RATE][2],
                                             v_ego, a_ego, lat_action_t)
-        desired_curvature = path_steering.apply_path_steering(desired_curvature, path, v_ego)
+        desired_curvature = apply_turn_correction(self.turn_correction, desired_curvature, self.turn_plan_curvature, path, v_ego)
+      else:
+        if turn_sign:
+          # the action head under-commits in slow turns while the model's own plan has the turn; use the plan as a floor
+          self.turn_plan_curvature = get_curvature_from_plan(plan[:, Plan.T_FROM_CURRENT_EULER][:, 2], plan[:, Plan.ORIENTATION_RATE][:, 2],
+                                                             self.constants.T_IDXS, v_ego, lat_action_t)
+          desired_curvature = apply_turn_plan_floor(desired_curvature, self.turn_plan_curvature, turn_sign)
+        if path_steering_enabled and path_steering.path_weight(v_ego) > 0.:
+          path = path_steering.path_curvature(plan[:, Plan.POSITION][:, 0], plan[:, Plan.POSITION][:, 1],
+                                              plan[:, Plan.T_FROM_CURRENT_EULER][:, 2], plan[0, Plan.ORIENTATION_RATE][2],
+                                              v_ego, a_ego, lat_action_t)
+          desired_curvature = path_steering.apply_path_steering(desired_curvature, path, v_ego)
     if blendful is not None:
       desired_curvature += blendful(desired_curvature)
 

openpilot/sunnypilot/modeld_v2/tests/test_turn_correction.py

@@ -0,0 +1,35 @@
+import pytest
+
+from openpilot.sunnypilot.modeld_v2 import turn_correction as tc
+
+C = tc.TurnCorrection('test', v_full=6., v_end=10., theta_path=0.1, theta_time=0.8)
+
+
+def test_no_change_at_road_speed_and_full_below_v_full():
+  assert tc.apply_turn_correction(C, 0.05, 0.10, 0.12, 12.) == 0.05
+  full = tc.apply_turn_correction(C, 0.05, 0.10, 0.12, 3.)
+  assert full == pytest.approx(0.05 + 0.1 * 0.07 + 0.8 * 0.05)
+  half = tc.apply_turn_correction(C, 0.05, 0.10, 0.12, 8.)
+  assert half == pytest.approx(0.05 + 0.5 * (full - 0.05))
+
+
+def test_it_stays_within_the_models_own_range():
+  # the plan's time axis says far more than both the action and the line at the car's speed: bounded at path + 10%
+  assert tc.apply_turn_correction(C, 0.10, 0.40, 0.14, 3.) == pytest.approx(0.14 + 0.1 * 0.04)
+  # a turn the action and the line both say is straight is not invented from the time axis
+  assert tc.apply_turn_correction(C, 0.0, 0.2, 0.0, 3.) == 0.0
+  # nor reversed: action and line right, the time axis left
+  assert tc.apply_turn_correction(C, 0.1, -0.2, 0.1, 3.) == pytest.approx(0.1)
+
+
+def test_it_can_lower_an_overcommitted_action_toward_the_line():
+  lower = tc.TurnCorrection('test', 4.5, 7., theta_path=0.9, theta_time=0.)
+  assert tc.apply_turn_correction(lower, -0.21, -0.21, -0.14, 3.) == pytest.approx(-0.21 + 0.9 * 0.07)
+
+
+def test_lookup_by_ref_and_local_prefix(monkeypatch):
+  monkeypatch.setattr(tc, 'CORRECTIONS', {'abc': C, 'local-ff_turns_600-': C})
+  assert tc.correction_for('abc') is C
+  assert tc.correction_for('local-ff_turns_600-0123456789abcdef') is C
+  assert tc.correction_for('local-other-0123') is None
+  assert tc.correction_for('') is None

openpilot/sunnypilot/modeld_v2/turn_correction.py

@@ -0,0 +1,56 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+from dataclasses import dataclass
+
+import numpy as np
+
+# Learned turn correction: per model, a small correction on top of its action toward the line FSD and the driver
+# took, fitted by openpilot.distill's supervised/fit_turn_correction.py on open-loop frames of driver/FSD turns and
+# closed-loop frames of the user's engaged drives (where the driver or FSD was steering, or a turn openpilot steered
+# with no push-back), scored leave-one-route-out. It replaces the 0.75 turn plan floor and the path cap for a model
+# that has one. With
+#   time = the plan's curvature at action_t on its time axis (get_curvature_from_plan)
+#   path = the plan's heading at the arc length the car covers in action_t (path_steering.path_curvature)
+# command = action + s(v) * (theta_path * (path - action) + theta_time * (time - action)), s = 1 below v_full and 0
+# from v_end, and the command stays within the range action..path spans (plus 10%), so it can only move the action
+# toward the model's own line, never invent or reverse a turn.
+MARGIN = 0.1
+
+
+@dataclass(frozen=True)
+class TurnCorrection:
+  name: str
+  v_full: float
+  v_end: float
+  theta_path: float
+  theta_time: float
+
+
+# bundle ref (or a local build's ref prefix ending in '-') -> correction
+CORRECTIONS: dict[str, TurnCorrection] = {
+  # Nov 9 2026, closed-loop frames of routes 98, 9a, 9e, 9f, b0 (26 turns, 3640 frames). Held out by route, mean
+  # |command - driven| 1-3 m/s 0.039 -> 0.021 (0.75 floor 0.029), 3-4.5 m/s 0.028 -> 0.014, 4.5-6 m/s 0.017 -> 0.008,
+  # 6-9 m/s 0.006 -> 0.004; peak command / peak driven per turn 0.74 (floor 0.83) -> 1.02 [10-90%: 0.84-1.26].
+  'b92fc4828664f8767bce5e92f1630ed319cbe509': TurnCorrection('Light Speed 2', v_full=6.0, v_end=10.0,
+                                                            theta_path=0.0554, theta_time=0.8708),
+}
+
+
+def correction_for(ref: str) -> TurnCorrection | None:
+  if ref in CORRECTIONS:
+    return CORRECTIONS[ref]
+  return next((c for key, c in CORRECTIONS.items() if key.endswith('-') and ref.startswith(key)), None)
+
+
+def apply_turn_correction(correction: TurnCorrection, action: float, time: float, path: float, v_ego: float) -> float:
+  weight = float(np.clip((correction.v_end - v_ego) / (correction.v_end - correction.v_full), 0., 1.))
+  if weight == 0.:
+    return action
+  raw = action + weight * (correction.theta_path * (path - action) + correction.theta_time * (time - action))
+  lo, hi = min(action, path), max(action, path)
+  margin = MARGIN * (hi - lo)
+  return float(np.clip(raw, lo - margin, hi + margin))