diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index 67ed08b8c5962..2b3c6049676f9 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -64,46 +64,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 * 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); }