fix: prediction to use variable kalman filter

a bug was found on `predictStateStep` methods

* intention: predict the future state of the variable kalman filter `ekf`
* bug: using the member kalman filter `ekf_`
* fix: get the state vector from the variable kalman filter `ekf`

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

Author: technolojin

perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp

@@ -316,7 +316,7 @@ bool BicycleMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) c
 
   // Current state vector X t
   Eigen::MatrixXd X_t(DIM, 1);
-  getStateVector(X_t);
+  ekf.getX(X_t);
 
   const double cos_yaw = std::cos(X_t(IDX::YAW) + X_t(IDX::SLIP));
   const double sin_yaw = std::sin(X_t(IDX::YAW) + X_t(IDX::SLIP));

perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp

@@ -272,7 +272,7 @@ bool CTRVMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) cons
 
   // Current state vector X t
   Eigen::MatrixXd X_t(DIM, 1);
-  getStateVector(X_t);
+  ekf.getX(X_t);
 
   const double cos_yaw = std::cos(X_t(IDX::YAW));
   const double sin_yaw = std::sin(X_t(IDX::YAW));

perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp

@@ -209,7 +209,7 @@ bool CVMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) const
 
   // Current state vector X t
   Eigen::MatrixXd X_t(DIM, 1);
-  getStateVector(X_t);
+  ekf.getX(X_t);
 
   // Predict state vector X t+1
   Eigen::MatrixXd X_next_t(DIM, 1);  // predicted state

perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp

@@ -50,9 +50,15 @@ bool MotionModel::predictState(const rclcpp::Time & time)
     return true;
   }
 
-  if (!predictStateStep(dt, ekf_)) return false;
-
-  // update last_update_time_
+  // multi-step prediction
+  // if dt is too large, shorten dt and repeat prediction
+  const uint32_t repeat = std::floor(dt / dt_max_) + 1;
+  const double dt_ = dt / repeat;
+  for (uint32_t i = 0; i < repeat; ++i) {
+    if (!predictStateStep(dt_, ekf_)) return false;
+    last_update_time_ += rclcpp::Duration::from_seconds(dt_);
+  }
+  // reset the last_update_time_ to the prediction time
   last_update_time_ = time;
   return true;
 }
@@ -63,17 +69,23 @@ bool MotionModel::getPredictedState(
   // check if the state is initialized
   if (!checkInitialized()) return false;
 
+  const double dt = getDeltaTime(time);
+  if (dt < 1e-6 /*1usec*/) {
+    // no prediction, return the current state
+    ekf_.getX(X);
+    ekf_.getP(P);
+    return true;
+  }
+
   // copy the predicted state and covariance
   KalmanFilter tmp_ekf_for_no_update = ekf_;
-
-  double dt = getDeltaTime(time);
-  if (dt < 0.0) {
-    // a naive way to handle the case when the required prediction time is in the past
-    dt = 0.0;
+  // multi-step prediction
+  // if dt is too large, shorten dt and repeat prediction
+  const uint32_t repeat = std::floor(dt / dt_max_) + 1;
+  const double dt_ = dt / repeat;
+  for (uint32_t i = 0; i < repeat; ++i) {
+    if (!predictStateStep(dt_, tmp_ekf_for_no_update)) return false;
   }
-
-  if (!predictStateStep(dt, tmp_ekf_for_no_update)) return false;
-
   tmp_ekf_for_no_update.getX(X);
   tmp_ekf_for_no_update.getP(P);
   return true;