even better lane width logic

Gernby · Jul 23, 2019 · a324a47 · a324a47
1 parent 31ead1c
commit a324a47
Showing 1 changed file with 3 additions and 3 deletions.
diff --git a/selfdrive/controls/lib/model_parser.py b/selfdrive/controls/lib/model_parser.py
@@ -45,9 +45,9 @@ def update(self, v_ego, md):
     self.lane_width_certainty += 0.05 * decay_rate * (lr_prob - self.lane_width_certainty)
     current_lane_width = abs(l_poly[3] - r_poly[3])
     self.lane_width_estimate += 0.005 * decay_rate * (current_lane_width - self.lane_width_estimate)
-    speed_lane_width = interp(v_ego, [0., 31.], [2.8, 3.5])
-    self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \
-                    (1 - self.lane_width_certainty) * speed_lane_width
+    speed_lane_width = interp(v_ego, [0., 31.], [3.0, 3.5])
+    self.lane_width = (self.lane_width_certainty * self.lane_width_estimate + 0.2 * speed_lane_width) / (0.2 + self.lane_width_certainty)
+    #self.lane_width = self.lane_width_certainty * self.lane_width_estimate + (1.0 - self.lane_width_certainty) * speed_lane_width
 
     self.lead_dist = md.lead.dist
     self.lead_prob = md.lead.prob