fix low speed lateral (#20053)

* fix low speed lateral

* update ref

* fix failing CI jobs

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>
old-commit-hash: 0c900c8ce11409ddf574f5b7ff7bbab8b5536c36

selfdrive/controls/lib/lateral_mpc/generator.cpp

@@ -38,10 +38,10 @@ int main( )
   h << yy;
 
   // Heading error
-  h << (v_ego + 1.0 ) * psi;
+  h << (v_ego + 5.0 ) * psi;
 
   // Angular rate error
-  h << (v_ego + 1.0) * 4 * curvature_rate;
+  h << (v_ego + 5.0) * 4 * curvature_rate;
 
   BMatrix Q(3,3); Q.setAll(true);
   // Q(0,0) = 1.0;
@@ -57,7 +57,7 @@ int main( )
   hN << yy;
 
   // Heading errors
-  hN << (2.0 * v_ego + 1.0 ) * psi;
+  hN << (2.0 * v_ego + 5.0 ) * psi;
 
   BMatrix QN(2,2); QN.setAll(true);
   // QN(0,0) = 1.0;

selfdrive/controls/lib/lateral_mpc/lateral_mpc.c

@@ -73,11 +73,11 @@ int run_mpc(state_t * x0, log_t * solution, double v_ego,
   }
   for (i = 0; i < N; i+= 1){
     acadoVariables.y[NY*i + 0] = target_y[i];
-    acadoVariables.y[NY*i + 1] = (v_ego + 1.0) * target_psi[i];
+    acadoVariables.y[NY*i + 1] = (v_ego + 5.0) * target_psi[i];
     acadoVariables.y[NY*i + 2] = 0.0;
   }
   acadoVariables.yN[0] = target_y[N];
-  acadoVariables.yN[1] = (2.0 * v_ego + 1.0) * target_psi[N];
+  acadoVariables.yN[1] = (2.0 * v_ego + 5.0) * target_psi[N];
 
   acadoVariables.x0[0] = x0->x;
   acadoVariables.x0[1] = x0->y;

selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:5ba1e5ddf6b2f2cedaf735f105951854666a75279b25b95e5fa64422964f4a79
+oid sha256:4d7de5aed6e5014ec35745f92356850f5b5d31b226cbcf93e9fafe0a4e29118c
 size 243595

selfdrive/controls/lib/lateral_planner.py

@@ -170,11 +170,10 @@ class LateralPlanner():
     heading_pts = np.interp(v_ego * self.t_idxs[:MPC_N+1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw)
     self.y_pts = y_pts
 
-    v_ego_mpc = max(v_ego, 5.0)  # avoid mpc roughness due to low speed
     assert len(y_pts) == MPC_N + 1
     assert len(heading_pts) == MPC_N + 1
     self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
-                        float(v_ego_mpc),
+                        float(v_ego),
                         CAR_ROTATION_RADIUS,
                         list(y_pts),
                         list(heading_pts))

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-0e2d73270816b93dfbd4c2f673591a7ba492edac
+cce762c643883cee11ae7683ddff20faa47b51b7