feat(ekf_localizer): input ekf_dt to simple1dfilter (autowarefoundati…

…on#8622) input ekf_dt to simple1dfilter Signed-off-by: Yamato Ando <[email protected]> Co-authored-by: TaikiYamada4 <[email protected]>
vividf · Aug 29, 2024 · 09eda62 · 09eda62
1 parent e709b10
commit 09eda62
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 14 deletions.
diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp
@@ -60,31 +60,27 @@ class Simple1DFilter
     var_ = 1e9;
     proc_var_x_c_ = 0.0;
   };
-  void init(const double init_obs, const double obs_var, const rclcpp::Time & time)
+  void init(const double init_obs, const double obs_var)
   {
     x_ = init_obs;
     var_ = obs_var;
-    latest_time_ = time;
     initialized_ = true;
   };
-  void update(const double obs, const double obs_var, const rclcpp::Time & time)
+  void update(const double obs, const double obs_var, const double dt)
   {
     if (!initialized_) {
-      init(obs, obs_var, time);
+      init(obs, obs_var);
       return;
     }
 
     // Prediction step (current variance)
-    double dt = (time - latest_time_).seconds();
     double proc_var_x_d = proc_var_x_c_ * dt * dt;
     var_ = var_ + proc_var_x_d;
 
     // Update step
     double kalman_gain = var_ / (var_ + obs_var);
     x_ = x_ + kalman_gain * (obs - x_);
     var_ = (1 - kalman_gain) * var_;
-
-    latest_time_ = time;
   };
   void set_proc_var(const double proc_var) { proc_var_x_c_ = proc_var; }
   [[nodiscard]] double get_x() const { return x_; }
@@ -95,7 +91,6 @@ class Simple1DFilter
   double x_;
   double var_;
   double proc_var_x_c_;
-  rclcpp::Time latest_time_;
 };
 
 class EKFLocalizer : public rclcpp::Node

diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp
@@ -456,9 +456,9 @@ void EKFLocalizer::update_simple_1d_filters(
   double pitch_var =
     pose.pose.covariance[COV_IDX::PITCH_PITCH] * static_cast<double>(smoothing_step);
 
-  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);
+  z_filter_.update(z, z_var, ekf_dt_);
+  roll_filter_.update(rpy.x, roll_var, ekf_dt_);
+  pitch_filter_.update(rpy.y, pitch_var, ekf_dt_);
 }
 
 void EKFLocalizer::init_simple_1d_filters(
@@ -473,9 +473,9 @@ void EKFLocalizer::init_simple_1d_filters(
   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_var, pose.header.stamp);
-  roll_filter_.init(rpy.x, roll_var, pose.header.stamp);
-  pitch_filter_.init(rpy.y, pitch_var, pose.header.stamp);
+  z_filter_.init(z, z_var);
+  roll_filter_.init(rpy.x, roll_var);
+  pitch_filter_.init(rpy.y, pitch_var);
 }
 
 /**