From 23e13fde3617938ab7e65c968d98470b9ec20274 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Fri, 23 Aug 2024 16:17:00 +0900 Subject: [PATCH] refactor(ekf_localizer): rename dev to var in the Simple1DFilter class (#8576) rename dev to var Signed-off-by: Yamato Ando --- .../include/ekf_localizer/ekf_localizer.hpp | 30 ++++++++-------- .../ekf_localizer/src/ekf_localizer.cpp | 36 +++++++++---------- 2 files changed, 33 insertions(+), 33 deletions(-) diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index 7e091a0e88a19..f3830c303ab48 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -56,44 +56,44 @@ class Simple1DFilter { initialized_ = false; x_ = 0; - dev_ = 1e9; - proc_dev_x_c_ = 0.0; + var_ = 1e9; + proc_var_x_c_ = 0.0; }; - void init(const double init_obs, const double obs_dev, const rclcpp::Time & time) + void init(const double init_obs, const double obs_var, const rclcpp::Time & time) { x_ = init_obs; - dev_ = obs_dev; + var_ = obs_var; latest_time_ = time; initialized_ = true; }; - void update(const double obs, const double obs_dev, const rclcpp::Time & time) + void update(const double obs, const double obs_var, const rclcpp::Time & time) { if (!initialized_) { - init(obs, obs_dev, time); + init(obs, obs_var, time); return; } - // Prediction step (current stddev_) + // Prediction step (current variance) double dt = (time - latest_time_).seconds(); - double proc_dev_x_d = proc_dev_x_c_ * dt * dt; - dev_ = dev_ + proc_dev_x_d; + double proc_var_x_d = proc_var_x_c_ * dt * dt; + var_ = var_ + proc_var_x_d; // Update step - double kalman_gain = dev_ / (dev_ + obs_dev); + double kalman_gain = var_ / (var_ + obs_var); x_ = x_ + kalman_gain * (obs - x_); - dev_ = (1 - kalman_gain) * dev_; + var_ = (1 - kalman_gain) * var_; latest_time_ = time; }; - void set_proc_dev(const double proc_dev) { proc_dev_x_c_ = proc_dev; } + void set_proc_var(const double proc_var) { proc_var_x_c_ = proc_var; } [[nodiscard]] double get_x() const { return x_; } - [[nodiscard]] double get_dev() const { return dev_; } + [[nodiscard]] double get_var() const { return var_; } private: bool initialized_; double x_; - double dev_; - double proc_dev_x_c_; + double var_; + double proc_var_x_c_; rclcpp::Time latest_time_; }; diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 090395398026e..707d5e585cb0c 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -100,9 +100,9 @@ EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options) ekf_module_ = std::make_unique(warning_, params_); logger_configure_ = std::make_unique(this); - z_filter_.set_proc_dev(params_.z_filter_proc_dev); - roll_filter_.set_proc_dev(params_.roll_filter_proc_dev); - pitch_filter_.set_proc_dev(params_.pitch_filter_proc_dev); + z_filter_.set_proc_var(params_.z_filter_proc_dev * params_.z_filter_proc_dev); + roll_filter_.set_proc_var(params_.roll_filter_proc_dev * params_.roll_filter_proc_dev); + pitch_filter_.set_proc_var(params_.pitch_filter_proc_dev * params_.pitch_filter_proc_dev); } /* @@ -367,9 +367,9 @@ void EKFLocalizer::publish_estimate_result( pose_cov.pose.covariance = ekf_module_->get_current_pose_covariance(); using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - pose_cov.pose.covariance[COV_IDX::Z_Z] = z_filter_.get_dev(); - pose_cov.pose.covariance[COV_IDX::ROLL_ROLL] = roll_filter_.get_dev(); - pose_cov.pose.covariance[COV_IDX::PITCH_PITCH] = pitch_filter_.get_dev(); + pose_cov.pose.covariance[COV_IDX::Z_Z] = z_filter_.get_var(); + pose_cov.pose.covariance[COV_IDX::ROLL_ROLL] = roll_filter_.get_var(); + pose_cov.pose.covariance[COV_IDX::PITCH_PITCH] = pitch_filter_.get_var(); pub_pose_cov_->publish(pose_cov); @@ -466,14 +466,14 @@ void EKFLocalizer::update_simple_1d_filters( const auto rpy = autoware::universe_utils::getRPY(pose.pose.pose.orientation); using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - double z_dev = pose.pose.covariance[COV_IDX::Z_Z] * static_cast(smoothing_step); - double roll_dev = pose.pose.covariance[COV_IDX::ROLL_ROLL] * static_cast(smoothing_step); - double pitch_dev = + double z_var = pose.pose.covariance[COV_IDX::Z_Z] * static_cast(smoothing_step); + double roll_var = pose.pose.covariance[COV_IDX::ROLL_ROLL] * static_cast(smoothing_step); + double pitch_var = pose.pose.covariance[COV_IDX::PITCH_PITCH] * static_cast(smoothing_step); - z_filter_.update(z, z_dev, pose.header.stamp); - roll_filter_.update(rpy.x, roll_dev, pose.header.stamp); - pitch_filter_.update(rpy.y, pitch_dev, pose.header.stamp); + z_filter_.update(z, z_var, pose.header.stamp); + roll_filter_.update(rpy.x, roll_var, pose.header.stamp); + pitch_filter_.update(rpy.y, pitch_var, pose.header.stamp); } void EKFLocalizer::init_simple_1d_filters( @@ -484,13 +484,13 @@ void EKFLocalizer::init_simple_1d_filters( const auto rpy = autoware::universe_utils::getRPY(pose.pose.pose.orientation); using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - double z_dev = pose.pose.covariance[COV_IDX::Z_Z]; - double roll_dev = pose.pose.covariance[COV_IDX::ROLL_ROLL]; - double pitch_dev = pose.pose.covariance[COV_IDX::PITCH_PITCH]; + double z_var = pose.pose.covariance[COV_IDX::Z_Z]; + double roll_var = pose.pose.covariance[COV_IDX::ROLL_ROLL]; + double pitch_var = pose.pose.covariance[COV_IDX::PITCH_PITCH]; - z_filter_.init(z, z_dev, pose.header.stamp); - roll_filter_.init(rpy.x, roll_dev, pose.header.stamp); - pitch_filter_.init(rpy.y, pitch_dev, pose.header.stamp); + z_filter_.init(z, z_var, pose.header.stamp); + roll_filter_.init(rpy.x, roll_var, pose.header.stamp); + pitch_filter_.init(rpy.y, pitch_var, pose.header.stamp); } /**