Removed proc_cov_*_d_ from EKFLocalizer

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>
autowarefoundation · Aug 27, 2024 · d781b4c · d781b4c
1 parent a64566e
commit d781b4c
Show file tree

Hide file tree

Showing 2 changed files with 0 additions and 15 deletions.
diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp
@@ -160,11 +160,6 @@ class EKFLocalizer : public rclcpp::Node
 
   double ekf_dt_;
 
-  /* process noise variance for discrete model */
-  double proc_cov_yaw_d_;  //!< @brief  discrete yaw process noise
-  double proc_cov_vx_d_;   //!< @brief  discrete process noise in d_vx=0
-  double proc_cov_wz_d_;   //!< @brief  discrete process noise in d_wz=0
-
   bool is_activated_;
 
   EKFDiagnosticInfo pose_diag_info_;

diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp
@@ -52,11 +52,6 @@ EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options)
   twist_queue_(params_.twist_smoothing_steps),
   last_angular_velocity_(0.0, 0.0, 0.0)
 {
-  /* convert to continuous to discrete */
-  proc_cov_vx_d_ = std::pow(params_.proc_stddev_vx_c * ekf_dt_, 2.0);
-  proc_cov_wz_d_ = std::pow(params_.proc_stddev_wz_c * ekf_dt_, 2.0);
-  proc_cov_yaw_d_ = std::pow(params_.proc_stddev_yaw_c * ekf_dt_, 2.0);
-
   is_activated_ = false;
 
   /* initialize ros system */
@@ -132,11 +127,6 @@ void EKFLocalizer::update_predict_frequency(const rclcpp::Time & current_time)
 
       /* Register dt and accumulate time delay */
       ekf_module_->accumulate_delay_time(ekf_dt_);
-
-      /* Update discrete proc_cov*/
-      proc_cov_vx_d_ = std::pow(params_.proc_stddev_vx_c * ekf_dt_, 2.0);
-      proc_cov_wz_d_ = std::pow(params_.proc_stddev_wz_c * ekf_dt_, 2.0);
-      proc_cov_yaw_d_ = std::pow(params_.proc_stddev_yaw_c * ekf_dt_, 2.0);
     }
   }
   last_predict_time_ = std::make_shared<const rclcpp::Time>(current_time);