fix(ekf_localizer): modified std deviation to deviation (autowarefoun…

…dation#1565)

* issue1562:modified std deviation to deviation

* ci(pre-commit): autofix

* Update localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp

Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com>

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com>

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_;
 };
 

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<tier4_debug_msgs::msg::Float64MultiArrayStamped>("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);
 }