fix(mpc): change the way to calculate target steering rate

Signed-off-by: Takamasa Horibe <[email protected]>
tier4 · Oct 17, 2023 · 5dbc881 · 5dbc881
1 parent 2dea1c3
commit 5dbc881
Showing 1 changed file with 2 additions and 3 deletions.
diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp
@@ -753,10 +753,9 @@ double MPC::calcDesiredSteeringRate(
 
   const size_t STEER_IDX = 2;  // for kinematics model
 
-  const auto steer_0 = x0(STEER_IDX, 0);
   const auto steer_1 = Xex(STEER_IDX, 0);
-
-  const auto steer_rate = (steer_1 - steer_0) / predict_dt;
+  const auto steer_2 = Xex(STEER_IDX + m_vehicle_model_ptr->getDimX(), 0);
+  const auto steer_rate = (steer_2 - steer_1) / predict_dt;
 
   return steer_rate;
 }