feat(mpc): update target steering rate calculation method

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

control/trajectory_follower/include/trajectory_follower/mpc.hpp

@@ -266,6 +266,14 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC
    * @brief add weights related to lateral_jerk, steering_rate, steering_acc into f
    */
   void addSteerWeightF(const float64_t predition_dt, Eigen::MatrixXd * f) const;
+
+  /**
+   * @brief calculate desired steering rate.
+   */
+  float64_t calcDesiredSteeringRate(
+    const MPCMatrix & m, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex,
+    const float64_t u_filtered, const float current_steer, const float64_t predict_dt) const;
+
   /**
    * @brief check if the matrix has invalid value
    */

control/trajectory_follower/src/mpc.cpp

@@ -94,8 +94,8 @@ bool8_t MPC::calculateMPC(
   /* set control command */
   {
     ctrl_cmd.steering_tire_angle = static_cast<float>(u_filtered);
-    ctrl_cmd.steering_tire_rotation_rate =
-      static_cast<float>((u_filtered - current_steer.steering_tire_angle) / prediction_dt);
+    ctrl_cmd.steering_tire_rotation_rate = static_cast<float>(calcDesiredSteeringRate(
+      mpc_matrix, x0, Uex, u_filtered, current_steer.steering_tire_angle, prediction_dt));
   }
 
   storeSteerCmd(u_filtered);
@@ -813,6 +813,28 @@ float64_t MPC::getPredictionDeletaTime(
   return std::max(dt, m_param.prediction_dt);
 }
 
+float64_t MPC::calcDesiredSteeringRate(
+  const MPCMatrix & mpc_matrix, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex,
+  const float64_t u_filtered, const float current_steer, const float64_t predict_dt) const
+{
+  if (m_vehicle_model_type != "kinematics") {
+    // not supported yet. Use old implementation.
+    return (u_filtered - current_steer) / predict_dt;
+  }
+
+  // calculate predicted states to get the steering motion
+  const auto & m = mpc_matrix;
+  const Eigen::MatrixXd Xex = m.Aex * x0 + m.Bex * Uex + m.Wex;
+
+  const size_t STEER_IDX = 2;  // for kinematics model
+  const size_t STATE_SIZE = m_vehicle_model_ptr->getDimX();
+
+  const auto steer_0 = Xex(STEER_IDX, 0);
+  const auto steer_1 = Xex(STEER_IDX + STATE_SIZE, 0);
+
+  return (steer_1 - steer_0) / m_param.prediction_dt;
+}
+
 bool8_t MPC::isValid(const MPCMatrix & m) const
 {
   if (