diff --git a/selfdrive/controls/lib/lateral_mpc/generator.cpp b/selfdrive/controls/lib/lateral_mpc/generator.cpp
index 09db8d9422..96488f4cc4 100644
--- a/selfdrive/controls/lib/lateral_mpc/generator.cpp
+++ b/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;
diff --git a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c
index fd45bf52cb..ff4fd4203d 100644
--- a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c
+++ b/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;
diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c
index 7a11392345..81a01da09d 100644
--- a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c
+++ b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c
@@ -86,15 +86,15 @@ const real_t* od = in + 5;
 
 /* Compute outputs: */
 out[0] = xd[1];
-out[1] = ((od[0]+(real_t)(1.0000000000000000e+00))*xd[2]);
-out[2] = (((od[0]+(real_t)(1.0000000000000000e+00))*(real_t)(4.0000000000000000e+00))*u[0]);
+out[1] = ((od[0]+(real_t)(5.0000000000000000e+00))*xd[2]);
+out[2] = (((od[0]+(real_t)(5.0000000000000000e+00))*(real_t)(4.0000000000000000e+00))*u[0]);
 out[3] = (real_t)(0.0000000000000000e+00);
 out[4] = (real_t)(1.0000000000000000e+00);
 out[5] = (real_t)(0.0000000000000000e+00);
 out[6] = (real_t)(0.0000000000000000e+00);
 out[7] = (real_t)(0.0000000000000000e+00);
 out[8] = (real_t)(0.0000000000000000e+00);
-out[9] = (od[0]+(real_t)(1.0000000000000000e+00));
+out[9] = (od[0]+(real_t)(5.0000000000000000e+00));
 out[10] = (real_t)(0.0000000000000000e+00);
 out[11] = (real_t)(0.0000000000000000e+00);
 out[12] = (real_t)(0.0000000000000000e+00);
@@ -102,7 +102,7 @@ out[13] = (real_t)(0.0000000000000000e+00);
 out[14] = (real_t)(0.0000000000000000e+00);
 out[15] = (real_t)(0.0000000000000000e+00);
 out[16] = (real_t)(0.0000000000000000e+00);
-out[17] = ((od[0]+(real_t)(1.0000000000000000e+00))*(real_t)(4.0000000000000000e+00));
+out[17] = ((od[0]+(real_t)(5.0000000000000000e+00))*(real_t)(4.0000000000000000e+00));
 }
 
 void acado_evaluateLSQEndTerm(const real_t* in, real_t* out)
@@ -112,14 +112,14 @@ const real_t* od = in + 4;
 
 /* Compute outputs: */
 out[0] = xd[1];
-out[1] = ((((real_t)(2.0000000000000000e+00)*od[0])+(real_t)(1.0000000000000000e+00))*xd[2]);
+out[1] = ((((real_t)(2.0000000000000000e+00)*od[0])+(real_t)(5.0000000000000000e+00))*xd[2]);
 out[2] = (real_t)(0.0000000000000000e+00);
 out[3] = (real_t)(1.0000000000000000e+00);
 out[4] = (real_t)(0.0000000000000000e+00);
 out[5] = (real_t)(0.0000000000000000e+00);
 out[6] = (real_t)(0.0000000000000000e+00);
 out[7] = (real_t)(0.0000000000000000e+00);
-out[8] = (((real_t)(2.0000000000000000e+00)*od[0])+(real_t)(1.0000000000000000e+00));
+out[8] = (((real_t)(2.0000000000000000e+00)*od[0])+(real_t)(5.0000000000000000e+00));
 out[9] = (real_t)(0.0000000000000000e+00);
 }
 
diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py
index d9efd894ac..b77be714ba 100644
--- a/selfdrive/controls/lib/lateral_planner.py
+++ b/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))
diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit
index 21d8f80378..645be12ff3 100644
--- a/selfdrive/test/process_replay/ref_commit
+++ b/selfdrive/test/process_replay/ref_commit
@@ -1 +1 @@
-0e2d73270816b93dfbd4c2f673591a7ba492edac
\ No newline at end of file
+cce762c643883cee11ae7683ddff20faa47b51b7
\ No newline at end of file