diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index 332d4e0837e54..d247c817874fb 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -60,22 +60,20 @@ 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; @@ -83,8 +81,6 @@ class Simple1DFilter 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 index 10c706f4032b6..e77a1156bf049 100644 --- 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(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); } /**