From 15c5f8add7b268c359eabceed52cf4bfdf5b98a4 Mon Sep 17 00:00:00 2001 From: TaikiYamada4 Date: Tue, 7 Nov 2023 11:13:57 +0900 Subject: [PATCH] Removed unused paramters proc_cov_vx_d_, proc_cov_wz_d_, proc_cov_yaw_d, and proc_cov_yaw_bias_d_ Signed-off-by: TaikiYamada4 --- .../include/ekf_localizer/ekf_localizer.hpp | 6 ------ localization/ekf_localizer/src/ekf_localizer.cpp | 10 ---------- 2 files changed, 16 deletions(-) diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index e2ffff2195645..eae431eca6d6a 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -158,12 +158,6 @@ class EKFLocalizer : public rclcpp::Node double ekf_rate_; double ekf_dt_; - /* process noise variance for discrete model */ - double proc_cov_yaw_d_; //!< @brief discrete yaw process noise - double proc_cov_yaw_bias_d_; //!< @brief discrete yaw bias 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 index 5dd78a00f2fbd..dbeef71fefdc5 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -49,11 +49,6 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti pose_queue_(params_.pose_smoothing_steps), twist_queue_(params_.twist_smoothing_steps) { - /* 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 */ @@ -112,11 +107,6 @@ void EKFLocalizer::updatePredictFrequency() ekf_rate_ = 1.0 / (get_clock()->now() - *last_predict_time_).seconds(); DEBUG_INFO(get_logger(), "[EKF] update ekf_rate_ to %f hz", ekf_rate_); ekf_dt_ = 1.0 / std::max(ekf_rate_, 0.1); - - /* 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(get_clock()->now());