Scene Complexity

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -357,7 +357,7 @@ class LongitudinalMpc:
     for i in range(N):
       self.solver.cost_set(i, 'Zl', Zl)
 
-  def set_weights(self, acceleration_jerk=1.0, danger_jerk=1.0, speed_jerk=1.0, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard, v_ego=0.0, lead_dist=50.0):
+  def set_weights(self, acceleration_jerk=1.0, danger_jerk=1.0, speed_jerk=1.0, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard, v_ego=0.0, lead_dist=50.0, uncertainty=0.0):
     # Update parameters based on current speed with interpolation for smooth scaling
     speed_mph = v_ego * CV.MS_TO_MPH  # Convert m/s to mph
 
@@ -389,6 +389,14 @@ class LongitudinalMpc:
     danger_jerk *= dist_factor
     speed_jerk *= dist_factor
 
+    # Scene complexity adjustment based on model uncertainty
+    complexity_factor = 1.0
+    filter_time_factor = 1.0
+
+    if uncertainty > 1.0:  # High uncertainty indicates complex scene
+      complexity_factor = 1.5  # Boost responsiveness
+      filter_time_factor = 0.0  # Disable smoothing for immediate response
+
     if self.mode == 'acc':
       a_change_cost = acceleration_jerk if prev_accel_constraint else 0
       cost_weights = [self.current_x_ego_cost, X_EGO_COST, V_EGO_COST, A_EGO_COST, a_change_cost, speed_jerk]
@@ -401,6 +409,15 @@ class LongitudinalMpc:
       raise NotImplementedError(f'Planner mode {self.mode} not recognized in planner cost set')
     self.set_cost_weights(cost_weights, constraint_cost_weights)
 
+    # Adjust filter time constants for complex scenes
+    if abs(filter_time_factor - getattr(self, 'prev_filter_time_factor', 1.0)) > 0.1:
+      current_a = self.lead_a_filter.x if hasattr(self.lead_a_filter, 'x') else 0.0
+      current_v = self.lead_v_filter.x if hasattr(self.lead_v_filter, 'x') else 0.0
+      new_filter_time = self.current_filter_time * filter_time_factor
+      self.lead_a_filter = FirstOrderFilter(current_a, new_filter_time, self.dt)
+      self.lead_v_filter = FirstOrderFilter(current_v, new_filter_time, self.dt)
+      self.prev_filter_time_factor = filter_time_factor
+
   def set_cur_state(self, v, a):
     v_prev = self.x0[1]
     self.x0[1] = v

selfdrive/controls/lib/longitudinal_planner.py

@@ -215,8 +215,32 @@ class LongitudinalPlanner:
     self.lead_two = sm['radarState'].leadTwo
 
     lead_dist = self.lead_one.dRel if self.lead_one.status else 50.0
+
+    # Calculate scene uncertainty from model desire prediction entropy and disengage predictions
+    uncertainty = 0.0
+    if hasattr(sm['modelV2'], 'meta'):
+      # Desire prediction entropy (maneuver uncertainty)
+      desire_entropy = 0.0
+      if hasattr(sm['modelV2'].meta, 'desirePrediction'):
+        desire_probs = sm['modelV2'].meta.desirePrediction
+        if len(desire_probs) > 1:
+          desire_probs = np.array(desire_probs)
+          desire_probs = desire_probs / np.sum(desire_probs)  # Normalize
+          desire_entropy = -np.sum(desire_probs * np.log(desire_probs + 1e-10))
+
+      # Disengage prediction risk (intervention likelihood)
+      disengage_risk = 0.0
+      if hasattr(sm['modelV2'].meta, 'disengagePredictions'):
+        # Use brake press probabilities as primary risk indicator
+        brake_probs = sm['modelV2'].meta.disengagePredictions.brakePressProbs
+        if len(brake_probs) > 0:
+          disengage_risk = np.max(brake_probs)  # Peak risk over time horizon
+
+      # Combined uncertainty metric
+      uncertainty = desire_entropy + disengage_risk
+
     self.mpc.set_weights(sm['frogpilotPlan'].accelerationJerk, sm['frogpilotPlan'].dangerJerk, sm['frogpilotPlan'].speedJerk, prev_accel_constraint,
-                         personality=sm['controlsState'].personality, v_ego=v_ego, lead_dist=lead_dist)
+                          personality=sm['controlsState'].personality, v_ego=v_ego, lead_dist=lead_dist, uncertainty=uncertainty)
     self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
     self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
     # After deciding the MPC mode via get_mpc_mode(), ensure MPC uses that mode when not mlsim