diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index 2dc06e50f8eb8..14c8347379af2 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -53,49 +53,6 @@ namespace autoware::ekf_localizer { -class Simple1DFilter -{ -public: - Simple1DFilter() - { - initialized_ = false; - x_ = 0; - var_ = 1e9; - proc_var_x_c_ = 0.0; - }; - void init(const double init_obs, const double obs_var) - { - x_ = init_obs; - var_ = obs_var; - initialized_ = true; - }; - void update(const double obs, const double obs_var, const double dt) - { - if (!initialized_) { - init(obs, obs_var); - return; - } - - // Prediction step (current variance) - 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_; - }; - void set_proc_var(const double proc_var) { proc_var_x_c_ = proc_var; } - [[nodiscard]] double get_x() const { return x_; } - [[nodiscard]] double get_var() const { return var_; } - -private: - bool initialized_; - double x_; - double var_; - double proc_var_x_c_; -}; - class EKFLocalizer : public rclcpp::Node { public: @@ -156,9 +113,6 @@ class EKFLocalizer : public rclcpp::Node //!< @brief extended kalman filter instance. std::unique_ptr ekf_module_; - Simple1DFilter z_filter_; - Simple1DFilter roll_filter_; - Simple1DFilter pitch_filter_; const HyperParameters params_; @@ -230,17 +184,6 @@ class EKFLocalizer : public rclcpp::Node void publish_callback_return_diagnostics( const std::string & callback_name, const rclcpp::Time & current_time); - /** - * @brief update simple 1d filter - */ - void update_simple_1d_filters( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose, const size_t smoothing_step); - - /** - * @brief initialize simple 1d filter - */ - void init_simple_1d_filters(const geometry_msgs::msg::PoseWithCovarianceStamped & pose); - /** * @brief trigger node */ @@ -250,11 +193,6 @@ class EKFLocalizer : public rclcpp::Node autoware::universe_utils::StopWatch stop_watch_; - /** - * @brief last angular velocity for compensating rph with delay - */ - tf2::Vector3 last_angular_velocity_; - friend class EKFLocalizerTestSuite; // for test code }; diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp index 5cf13dfb309b1..0659e3f73315b 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp @@ -49,6 +49,49 @@ struct EKFDiagnosticInfo double mahalanobis_distance{0.0}; }; +class Simple1DFilter +{ +public: + Simple1DFilter() + { + initialized_ = false; + x_ = 0; + var_ = 1e9; + proc_var_x_c_ = 0.0; + }; + void init(const double init_obs, const double obs_var) + { + x_ = init_obs; + var_ = obs_var; + initialized_ = true; + }; + void update(const double obs, const double obs_var, const double dt) + { + if (!initialized_) { + init(obs, obs_var); + return; + } + + // Prediction step (current variance) + 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_; + }; + void set_proc_var(const double proc_var) { proc_var_x_c_ = proc_var; } + [[nodiscard]] double get_x() const { return x_; } + [[nodiscard]] double get_var() const { return var_; } + +private: + bool initialized_; + double x_; + double var_; + double proc_var_x_c_; +}; + class EKFModule { private: @@ -65,8 +108,7 @@ class EKFModule const geometry_msgs::msg::TransformStamped & transform); [[nodiscard]] geometry_msgs::msg::PoseStamped get_current_pose( - const rclcpp::Time & current_time, const double z, const double roll, const double pitch, - bool get_biased_yaw) const; + const rclcpp::Time & current_time, bool get_biased_yaw) const; [[nodiscard]] geometry_msgs::msg::TwistStamped get_current_twist( const rclcpp::Time & current_time) const; [[nodiscard]] double get_yaw_bias() const; @@ -87,12 +129,26 @@ class EKFModule const PoseWithCovariance & pose, tf2::Vector3 last_angular_velocity, const double delay_time); private: + void update_simple_1d_filters( + const geometry_msgs::msg::PoseWithCovarianceStamped & pose, const size_t smoothing_step); + TimeDelayKalmanFilter kalman_filter_; std::shared_ptr warning_; const int dim_x_; std::vector accumulated_delay_times_; const HyperParameters params_; + + Simple1DFilter z_filter_; + Simple1DFilter roll_filter_; + Simple1DFilter pitch_filter_; + + /** + * @brief last angular velocity for compensating rph with delay + */ + tf2::Vector3 last_angular_velocity_; + + double ekf_dt_; }; } // namespace autoware::ekf_localizer diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 7879ddbc55caa..6a3fd2265c637 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -52,8 +52,7 @@ EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options) params_(this), ekf_dt_(params_.ekf_dt), pose_queue_(params_.pose_smoothing_steps), - twist_queue_(params_.twist_smoothing_steps), - last_angular_velocity_(0.0, 0.0, 0.0) + twist_queue_(params_.twist_smoothing_steps) { is_activated_ = false; @@ -99,10 +98,6 @@ EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options) ekf_module_ = std::make_unique(warning_, params_); logger_configure_ = std::make_unique(this); - - z_filter_.set_proc_var(params_.z_filter_proc_dev * params_.z_filter_proc_dev); - roll_filter_.set_proc_var(params_.roll_filter_proc_dev * params_.roll_filter_proc_dev); - pitch_filter_.set_proc_var(params_.pitch_filter_proc_dev * params_.pitch_filter_proc_dev); } /* @@ -182,14 +177,6 @@ void EKFLocalizer::timer_callback() bool is_updated = ekf_module_->measurement_update_pose(*pose, current_time, pose_diag_info_); if (is_updated) { pose_is_updated = true; - - // Update Simple 1D filter with considering change of roll, pitch and height (position z) - // values due to measurement pose delay - const double delay_time = - (current_time - pose->header.stamp).seconds() + params_.pose_additional_delay; - auto pose_with_rph_delay_compensation = - ekf_module_->compensate_rph_with_delay(*pose, last_angular_velocity_, delay_time); - update_simple_1d_filters(pose_with_rph_delay_compensation, params_.pose_smoothing_steps); } } DEBUG_INFO( @@ -220,10 +207,6 @@ void EKFLocalizer::timer_callback() ekf_module_->measurement_update_twist(*twist, current_time, twist_diag_info_); if (is_updated) { twist_is_updated = true; - last_angular_velocity_ = tf2::Vector3( - twist->twist.twist.angular.x, twist->twist.twist.angular.y, twist->twist.twist.angular.z); - } else { - last_angular_velocity_ = tf2::Vector3(0.0, 0.0, 0.0); } } DEBUG_INFO( @@ -232,13 +215,10 @@ void EKFLocalizer::timer_callback() } twist_diag_info_.no_update_count = twist_is_updated ? 0 : (twist_diag_info_.no_update_count + 1); - const double z = z_filter_.get_x(); - const double roll = roll_filter_.get_x(); - const double pitch = pitch_filter_.get_x(); const geometry_msgs::msg::PoseStamped current_ekf_pose = - ekf_module_->get_current_pose(current_time, z, roll, pitch, false); + ekf_module_->get_current_pose(current_time, false); const geometry_msgs::msg::PoseStamped current_biased_ekf_pose = - ekf_module_->get_current_pose(current_time, z, roll, pitch, true); + ekf_module_->get_current_pose(current_time, true); const geometry_msgs::msg::TwistStamped current_ekf_twist = ekf_module_->get_current_twist(current_time); @@ -260,15 +240,11 @@ void EKFLocalizer::timer_tf_callback() return; } - const double z = z_filter_.get_x(); - const double roll = roll_filter_.get_x(); - const double pitch = pitch_filter_.get_x(); - const rclcpp::Time current_time = this->now(); geometry_msgs::msg::TransformStamped transform_stamped; transform_stamped = autoware::universe_utils::pose2transform( - ekf_module_->get_current_pose(current_time, z, roll, pitch, false), "base_link"); + ekf_module_->get_current_pose(current_time, false), "base_link"); transform_stamped.header.stamp = current_time; tf_br_->sendTransform(transform_stamped); } @@ -305,7 +281,6 @@ void EKFLocalizer::callback_initial_pose( params_.pose_frame_id.c_str(), msg->header.frame_id.c_str()); } ekf_module_->initialize(*msg, transform); - init_simple_1d_filters(*msg); } /* @@ -357,12 +332,6 @@ void EKFLocalizer::publish_estimate_result( pose_cov.header.frame_id = current_ekf_pose.header.frame_id; pose_cov.pose.pose = current_ekf_pose.pose; pose_cov.pose.covariance = ekf_module_->get_current_pose_covariance(); - - using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - pose_cov.pose.covariance[COV_IDX::Z_Z] = z_filter_.get_var(); - pose_cov.pose.covariance[COV_IDX::ROLL_ROLL] = roll_filter_.get_var(); - pose_cov.pose.covariance[COV_IDX::PITCH_PITCH] = pitch_filter_.get_var(); - pub_pose_cov_->publish(pose_cov); geometry_msgs::msg::PoseWithCovarianceStamped biased_pose_cov = pose_cov; @@ -469,41 +438,6 @@ void EKFLocalizer::publish_callback_return_diagnostics( pub_diag_->publish(diag_msg); } -void EKFLocalizer::update_simple_1d_filters( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose, const size_t smoothing_step) -{ - double z = pose.pose.pose.position.z; - - const auto rpy = autoware::universe_utils::getRPY(pose.pose.pose.orientation); - - using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - double z_var = pose.pose.covariance[COV_IDX::Z_Z] * static_cast(smoothing_step); - double roll_var = pose.pose.covariance[COV_IDX::ROLL_ROLL] * static_cast(smoothing_step); - double pitch_var = - pose.pose.covariance[COV_IDX::PITCH_PITCH] * static_cast(smoothing_step); - - 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( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose) -{ - double z = pose.pose.pose.position.z; - - const auto rpy = autoware::universe_utils::getRPY(pose.pose.pose.orientation); - - using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - double z_var = pose.pose.covariance[COV_IDX::Z_Z]; - 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); - roll_filter_.init(rpy.x, roll_var); - pitch_filter_.init(rpy.y, pitch_var); -} - /** * @brief trigger node */ diff --git a/localization/ekf_localizer/src/ekf_module.cpp b/localization/ekf_localizer/src/ekf_module.cpp index d2c29c458572e..f00608e0bb6a4 100644 --- a/localization/ekf_localizer/src/ekf_module.cpp +++ b/localization/ekf_localizer/src/ekf_module.cpp @@ -40,7 +40,8 @@ EKFModule::EKFModule(std::shared_ptr warning, const HyperParameters & p : warning_(std::move(warning)), dim_x_(6), // x, y, yaw, yaw_bias, vx, wz accumulated_delay_times_(params.extend_state_step, 1.0E15), - params_(params) + params_(params), + last_angular_velocity_(0.0, 0.0, 0.0) { Eigen::MatrixXd x = Eigen::MatrixXd::Zero(dim_x_, 1); Eigen::MatrixXd p = Eigen::MatrixXd::Identity(dim_x_, dim_x_) * 1.0E15; // for x & y @@ -52,6 +53,9 @@ EKFModule::EKFModule(std::shared_ptr warning, const HyperParameters & p p(IDX::WZ, IDX::WZ) = 50.0; // for wz kalman_filter_.init(x, p, static_cast(params_.extend_state_step)); + z_filter_.set_proc_var(params_.z_filter_proc_dev * params_.z_filter_proc_dev); + roll_filter_.set_proc_var(params_.roll_filter_proc_dev * params_.roll_filter_proc_dev); + pitch_filter_.set_proc_var(params_.pitch_filter_proc_dev * params_.pitch_filter_proc_dev); } void EKFModule::initialize( @@ -80,12 +84,27 @@ void EKFModule::initialize( p(IDX::WZ, IDX::WZ) = 0.01; kalman_filter_.init(x, p, static_cast(params_.extend_state_step)); + + const double z = initial_pose.pose.pose.position.z; + + const auto rpy = autoware::universe_utils::getRPY(initial_pose.pose.pose.orientation); + + const double z_var = initial_pose.pose.covariance[COV_IDX::Z_Z]; + const double roll_var = initial_pose.pose.covariance[COV_IDX::ROLL_ROLL]; + const double pitch_var = initial_pose.pose.covariance[COV_IDX::PITCH_PITCH]; + + z_filter_.init(z, z_var); + roll_filter_.init(rpy.x, roll_var); + pitch_filter_.init(rpy.y, pitch_var); } geometry_msgs::msg::PoseStamped EKFModule::get_current_pose( - const rclcpp::Time & current_time, const double z, const double roll, const double pitch, - bool get_biased_yaw) const + const rclcpp::Time & current_time, bool get_biased_yaw) const { + const double z = z_filter_.get_x(); + const double roll = roll_filter_.get_x(); + const double pitch = pitch_filter_.get_x(); + const double x = kalman_filter_.getXelement(IDX::X); const double y = kalman_filter_.getXelement(IDX::Y); /* @@ -127,7 +146,15 @@ geometry_msgs::msg::TwistStamped EKFModule::get_current_twist( std::array EKFModule::get_current_pose_covariance() const { - return ekf_covariance_to_pose_message_covariance(kalman_filter_.getLatestP()); + std::array cov = + ekf_covariance_to_pose_message_covariance(kalman_filter_.getLatestP()); + + using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + cov[COV_IDX::Z_Z] = z_filter_.get_var(); + cov[COV_IDX::ROLL_ROLL] = roll_filter_.get_var(); + cov[COV_IDX::PITCH_PITCH] = pitch_filter_.get_var(); + + return cov; } std::array EKFModule::get_current_twist_covariance() const @@ -195,6 +222,7 @@ void EKFModule::predict_with_delay(const double dt) const Matrix6d a = create_state_transition_matrix(x_curr, dt); const Matrix6d q = process_noise_covariance(proc_cov_yaw_d, proc_cov_vx_d, proc_cov_wz_d); kalman_filter_.predictWithDelay(x_next, a, q); + ekf_dt_ = dt; } bool EKFModule::measurement_update_pose( @@ -277,6 +305,12 @@ bool EKFModule::measurement_update_pose( kalman_filter_.updateWithDelay(y, c, r, static_cast(delay_step)); + // Update Simple 1D filter with considering change of roll, pitch and height (position z) + // values due to measurement pose delay + auto pose_with_rph_delay_compensation = + compensate_rph_with_delay(pose, last_angular_velocity_, delay_time); + update_simple_1d_filters(pose_with_rph_delay_compensation, params_.pose_smoothing_steps); + // debug const Eigen::MatrixXd x_result = kalman_filter_.getLatestX(); DEBUG_PRINT_MAT(x_result.transpose()); @@ -328,6 +362,8 @@ bool EKFModule::measurement_update_twist( warning_->warn_throttle("twist frame_id must be base_link", 2000); } + last_angular_velocity_ = tf2::Vector3(0.0, 0.0, 0.0); + const Eigen::MatrixXd x_curr = kalman_filter_.getLatestX(); DEBUG_PRINT_MAT(x_curr.transpose()); @@ -388,6 +424,9 @@ bool EKFModule::measurement_update_twist( kalman_filter_.updateWithDelay(y, c, r, static_cast(delay_step)); + last_angular_velocity_ = tf2::Vector3( + twist.twist.twist.angular.x, twist.twist.twist.angular.y, twist.twist.twist.angular.z); + // debug const Eigen::MatrixXd x_result = kalman_filter_.getLatestX(); DEBUG_PRINT_MAT(x_result.transpose()); @@ -396,4 +435,22 @@ bool EKFModule::measurement_update_twist( return true; } +void EKFModule::update_simple_1d_filters( + const geometry_msgs::msg::PoseWithCovarianceStamped & pose, const size_t smoothing_step) +{ + double z = pose.pose.pose.position.z; + + const auto rpy = autoware::universe_utils::getRPY(pose.pose.pose.orientation); + + using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + double z_var = pose.pose.covariance[COV_IDX::Z_Z] * static_cast(smoothing_step); + double roll_var = pose.pose.covariance[COV_IDX::ROLL_ROLL] * static_cast(smoothing_step); + double pitch_var = + pose.pose.covariance[COV_IDX::PITCH_PITCH] * static_cast(smoothing_step); + + 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_); +} + } // namespace autoware::ekf_localizer