refactor(ekf_localizer): remove the prediction function to improve re…

…ferential transparency (#2269) * Remove 'predictKinematicsModel' * Move the model description to state_transition.cpp Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
autowarefoundation · Nov 30, 2022 · a85643f · a85643f
1 parent 8e13ced
commit a85643f
Show file tree

Hide file tree

Showing 2 changed files with 39 additions and 48 deletions.
diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp
@@ -151,7 +151,24 @@ void EKFLocalizer::timerCallback()
   /* predict model in EKF */
   stop_watch_.tic();
   DEBUG_INFO(get_logger(), "------------------------- start prediction -------------------------");
-  predictKinematicsModel();
+
+  const Eigen::MatrixXd X_curr = ekf_.getLatestX();
+  DEBUG_PRINT_MAT(X_curr.transpose());
+
+  const Eigen::MatrixXd P_curr = ekf_.getLatestP();
+
+  const double dt = ekf_dt_;
+
+  const Vector6d X_next = predictNextState(X_curr, dt);
+  const Matrix6d A = createStateTransitionMatrix(X_curr, dt);
+  const Matrix6d Q = processNoiseCovariance(proc_cov_yaw_d_, proc_cov_vx_d_, proc_cov_wz_d_);
+
+  ekf_.predictWithDelay(X_next, A, Q);
+
+  // debug
+  const Eigen::MatrixXd X_result = ekf_.getLatestX();
+  DEBUG_PRINT_MAT(X_result.transpose());
+  DEBUG_PRINT_MAT((X_result - X_curr).transpose());
   DEBUG_INFO(get_logger(), "[EKF] predictKinematicsModel calc time = %f [ms]", stop_watch_.toc());
   DEBUG_INFO(get_logger(), "------------------------- end prediction -------------------------\n");
 
@@ -380,52 +397,6 @@ void EKFLocalizer::initEKF()
   ekf_.init(X, P, params_.extend_state_step);
 }
 
-/*
- * predictKinematicsModel
- */
-void EKFLocalizer::predictKinematicsModel()
-{
-  /*  == Nonlinear model ==
-   *
-   * x_{k+1}   = x_k + vx_k * cos(yaw_k + b_k) * dt
-   * y_{k+1}   = y_k + vx_k * sin(yaw_k + b_k) * dt
-   * yaw_{k+1} = yaw_k + (wz_k) * dt
-   * b_{k+1}   = b_k
-   * vx_{k+1}  = vz_k
-   * wz_{k+1}  = wz_k
-   *
-   * (b_k : yaw_bias_k)
-   */
-
-  /*  == Linearized model ==
-   *
-   * A = [ 1, 0, -vx*sin(yaw+b)*dt, -vx*sin(yaw+b)*dt, cos(yaw+b)*dt,  0]
-   *     [ 0, 1,  vx*cos(yaw+b)*dt,  vx*cos(yaw+b)*dt, sin(yaw+b)*dt,  0]
-   *     [ 0, 0,                 1,                 0,             0, dt]
-   *     [ 0, 0,                 0,                 1,             0,  0]
-   *     [ 0, 0,                 0,                 0,             1,  0]
-   *     [ 0, 0,                 0,                 0,             0,  1]
-   */
-
-  const Eigen::MatrixXd X_curr = ekf_.getLatestX();
-  DEBUG_PRINT_MAT(X_curr.transpose());
-
-  const Eigen::MatrixXd P_curr = ekf_.getLatestP();
-
-  const double dt = ekf_dt_;
-
-  const Vector6d X_next = predictNextState(X_curr, dt);
-  const Matrix6d A = createStateTransitionMatrix(X_curr, dt);
-  const Matrix6d Q = processNoiseCovariance(proc_cov_yaw_d_, proc_cov_vx_d_, proc_cov_wz_d_);
-
-  ekf_.predictWithDelay(X_next, A, Q);
-
-  // debug
-  const Eigen::MatrixXd X_result = ekf_.getLatestX();
-  DEBUG_PRINT_MAT(X_result.transpose());
-  DEBUG_PRINT_MAT((X_result - X_curr).transpose());
-}
-
 /*
  * measurementUpdatePose
  */

diff --git a/localization/ekf_localizer/src/state_transition.cpp b/localization/ekf_localizer/src/state_transition.cpp
@@ -24,6 +24,18 @@ double normalizeYaw(const double & yaw)
   return std::atan2(std::sin(yaw), std::cos(yaw));
 }
 
+/*  == Nonlinear model ==
+ *
+ * x_{k+1}   = x_k + vx_k * cos(yaw_k + b_k) * dt
+ * y_{k+1}   = y_k + vx_k * sin(yaw_k + b_k) * dt
+ * yaw_{k+1} = yaw_k + (wz_k) * dt
+ * b_{k+1}   = b_k
+ * vx_{k+1}  = vz_k
+ * wz_{k+1}  = wz_k
+ *
+ * (b_k : yaw_bias_k)
+ */
+
 Vector6d predictNextState(const Vector6d & X_curr, const double dt)
 {
   const double x = X_curr(IDX::X);
@@ -43,7 +55,15 @@ Vector6d predictNextState(const Vector6d & X_curr, const double dt)
   return X_next;
 }
 
-// TODO(TakeshiIshita) show where the equation come from
+/*  == Linearized model ==
+ *
+ * A = [ 1, 0, -vx*sin(yaw+b)*dt, -vx*sin(yaw+b)*dt, cos(yaw+b)*dt,  0]
+ *     [ 0, 1,  vx*cos(yaw+b)*dt,  vx*cos(yaw+b)*dt, sin(yaw+b)*dt,  0]
+ *     [ 0, 0,                 1,                 0,             0, dt]
+ *     [ 0, 0,                 0,                 1,             0,  0]
+ *     [ 0, 0,                 0,                 0,             1,  0]
+ *     [ 0, 0,                 0,                 0,             0,  1]
+ */
 Matrix6d createStateTransitionMatrix(const Vector6d & X_curr, const double dt)
 {
   const double yaw = X_curr(IDX::YAW);