Fix lane width (#2520)

* use prob modifiers in lane width calc

* update ref

* update ref

selfdrive/controls/lib/lane_planner.py

@@ -68,13 +68,6 @@ class LanePlanner:
     self.r_poly[3] += CAMERA_OFFSET
 
     # Find current lanewidth
-    self.lane_width_certainty += 0.05 * (self.l_prob * self.r_prob - self.lane_width_certainty)
-    current_lane_width = abs(self.l_poly[3] - self.r_poly[3])
-    self.lane_width_estimate += 0.005 * (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
-
     # This will improve behaviour when lanes suddenly widen
     # these numbers were tested on 2000 segments and found to work well
     l_prob, r_prob = self.l_prob, self.r_prob
@@ -94,6 +87,13 @@ class LanePlanner:
     l_prob *= l_std_mod
     r_prob *= r_std_mod
 
+    self.lane_width_certainty += 0.05 * (l_prob * r_prob - self.lane_width_certainty)
+    current_lane_width = abs(self.l_poly[3] - self.r_poly[3])
+    self.lane_width_estimate += 0.005 * (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
+
     clipped_lane_width = min(4.0, self.lane_width)
     path_from_left_lane = self.l_poly.copy()
     path_from_left_lane[3] -= clipped_lane_width / 2.0

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-2d717d808d568c2a3939627430fc56747c52e5bd
+b4a8c7c439c2430082ffb49ed675ada8f3bab8e6