From d7d5c0d936ad2ad125c7b3cbcb66e1b7391613c5 Mon Sep 17 00:00:00 2001 From: Cynthia Liu Date: Wed, 10 Aug 2022 17:59:58 +0800 Subject: [PATCH 1/3] issue1562:modified std deviation to deviation --- .../include/ekf_localizer/ekf_localizer.hpp | 27 ++++++++++--------- .../ekf_localizer/src/ekf_localizer.cpp | 18 ++++++------- 2 files changed, 23 insertions(+), 22 deletions(-) diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index 67ed08b8c5962..70acceb173f68 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -57,6 +57,7 @@ struct TwistInfo int smoothing_steps; }; + class Simple1DFilter { public: @@ -64,46 +65,46 @@ class Simple1DFilter { initialized_ = false; x_ = 0; - stddev_ = 1e9; - proc_stddev_x_c_ = 0.0; + dev_ = 1e9; + proc_dev_x_c_ = 0.0; return; }; - void init(const double init_obs, const double obs_stddev, const rclcpp::Time time) + void init(const double init_obs, const double obs_dev, const rclcpp::Time time) { x_ = init_obs; - stddev_ = obs_stddev; + dev_ = obs_dev; latest_time_ = time; initialized_ = true; return; }; - void update(const double obs, const double obs_stddev, const rclcpp::Time time) + void update(const double obs, const double obs_dev, const rclcpp::Time time) { if (!initialized_) { - init(obs, obs_stddev, time); + init(obs, obs_dev, time); return; } // Prediction step (current stddev_) double dt = (time - latest_time_).seconds(); - double proc_stddev_x_d = proc_stddev_x_c_ * dt; - stddev_ = std::sqrt(stddev_ * stddev_ + proc_stddev_x_d * proc_stddev_x_d); + double proc_dev_x_d = proc_dev_x_c_ * dt; + dev_ = dev_ + proc_dev_x_d; // Update step - double kalman_gain = stddev_ * stddev_ / (stddev_ * stddev_ + obs_stddev * obs_stddev); + double kalman_gain = dev_ / (dev_ + obs_dev); x_ = x_ + kalman_gain * (obs - x_); - stddev_ = std::sqrt(1 - kalman_gain) * stddev_; + dev_ = (1 - kalman_gain)*dev_; latest_time_ = time; return; }; - void set_proc_stddev(const double proc_stddev) { proc_stddev_x_c_ = proc_stddev; } + void set_proc_dev(const double proc_dev) { proc_dev_x_c_ = proc_dev; } double get_x() { return x_; } private: bool initialized_; double x_; - double stddev_; - double proc_stddev_x_c_; + double dev_; + double proc_dev_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 93b2ccd18654c..645fe5b825609 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -106,9 +106,9 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti initEKF(); - z_filter_.set_proc_stddev(1.0); - roll_filter_.set_proc_stddev(0.1); - pitch_filter_.set_proc_stddev(0.1); + z_filter_.set_proc_dev(1.0); + roll_filter_.set_proc_dev(0.01); + pitch_filter_.set_proc_dev(0.01); /* debug */ pub_debug_ = create_publisher("debug", 1); @@ -764,11 +764,11 @@ void EKFLocalizer::updateSimple1DFilters(const geometry_msgs::msg::PoseWithCovar tf2::fromMsg(pose.pose.pose.orientation, q_tf); tf2::Matrix3x3(q_tf).getRPY(roll, pitch, yaw_tmp); - double z_stddev = std::sqrt(pose.pose.covariance[2 * 6 + 2]); - double roll_stddev = std::sqrt(pose.pose.covariance[3 * 6 + 3]); - double pitch_stddev = std::sqrt(pose.pose.covariance[4 * 6 + 4]); + double z_dev = pose.pose.covariance[2 * 6 + 2]; + double roll_dev = pose.pose.covariance[3 * 6 + 3]; + double pitch_dev = pose.pose.covariance[4 * 6 + 4]; - z_filter_.update(z, z_stddev, pose.header.stamp); - roll_filter_.update(roll, roll_stddev, pose.header.stamp); - pitch_filter_.update(pitch, pitch_stddev, pose.header.stamp); + z_filter_.update(z, z_dev, pose.header.stamp); + roll_filter_.update(roll, roll_dev, pose.header.stamp); + pitch_filter_.update(pitch, pitch_dev, pose.header.stamp); } From 90443a4eafb7294cab170fdabedc317b67676e85 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 10 Aug 2022 10:54:15 +0000 Subject: [PATCH 2/3] ci(pre-commit): autofix --- .../ekf_localizer/include/ekf_localizer/ekf_localizer.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index 70acceb173f68..11e306c819791 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -57,7 +57,6 @@ struct TwistInfo int smoothing_steps; }; - class Simple1DFilter { public: @@ -92,7 +91,7 @@ class Simple1DFilter // Update step double kalman_gain = dev_ / (dev_ + obs_dev); x_ = x_ + kalman_gain * (obs - x_); - dev_ = (1 - kalman_gain)*dev_; + dev_ = (1 - kalman_gain) * dev_; latest_time_ = time; return; From d1a97743eb68d6845603c4794fce2eac231913eb Mon Sep 17 00:00:00 2001 From: cyn-liu <104069308+cyn-liu@users.noreply.github.com> Date: Mon, 15 Aug 2022 15:17:05 +0800 Subject: [PATCH 3/3] Update localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> --- .../ekf_localizer/include/ekf_localizer/ekf_localizer.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index 11e306c819791..2b3c6049676f9 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -85,7 +85,7 @@ class Simple1DFilter // Prediction step (current stddev_) double dt = (time - latest_time_).seconds(); - double proc_dev_x_d = proc_dev_x_c_ * dt; + double proc_dev_x_d = proc_dev_x_c_ * dt * dt; dev_ = dev_ + proc_dev_x_d; // Update step