From 86d66c99193c87970a1b73fd6118911f3397342b Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Mon, 4 Jul 2022 18:01:43 +0900 Subject: [PATCH] feat(ekf_localizer): allow multi sensor inputs in ekf_localizer (#1027) * first commit Signed-off-by: kminoda * ci(pre-commit): autofix Signed-off-by: kminoda * updated Signed-off-by: kminoda * deque to queue Signed-off-by: kminoda * ci(pre-commit): autofix Signed-off-by: kminoda * queue debugged Signed-off-by: kminoda * ci(pre-commit): autofix Signed-off-by: kminoda * ci(pre-commit): autofix Signed-off-by: kminoda * deque to queue Signed-off-by: kminoda * queue didn't support q.clear()... Signed-off-by: kminoda * for debug, and must be ignored later Signed-off-by: kminoda * ci(pre-commit): autofix * removed dummy variables Signed-off-by: kminoda * ci(pre-commit): autofix * run pre-commit Signed-off-by: kminoda * update readme Signed-off-by: kminoda * update readme Signed-off-by: kminoda * ci(pre-commit): autofix * reflected some review comments Signed-off-by: kminoda * reflected some review comments Signed-off-by: kminoda * added smoothing_steps param in pose_info and twist_info Signed-off-by: kminoda * ci(pre-commit): autofix * use withcovariance in PoseInfo & TwistInfo, now build works Signed-off-by: kminoda * ci(pre-commit): autofix * (not verified yet) update z, roll, pitch using 1D filter Signed-off-by: kminoda * ci(pre-commit): autofix * added TODO comments Signed-off-by: kminoda * ci(pre-commit): autofix * update initialization of simple1DFilter Signed-off-by: kminoda * fixed a bug (=NDT did not converge when launching logging_simulator) Signed-off-by: kminoda * debug Signed-off-by: kminoda * change gnss covariance, may have to be removed from PR Signed-off-by: kminoda * ci(pre-commit): autofix * removed unnecessary comments Signed-off-by: kminoda * added known issue Signed-off-by: kminoda * ci(pre-commit): autofix * change the default gnss covariance to the previous one Signed-off-by: kminoda * pre-commit Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../pose_twist_fusion_filter.launch.xml | 2 +- localization/ekf_localizer/README.md | 74 ++++----- .../include/ekf_localizer/ekf_localizer.hpp | 84 ++++++++++- .../launch/ekf_localizer.launch.xml | 8 +- .../ekf_localizer/src/ekf_localizer.cpp | 142 +++++++++++------- 5 files changed, 213 insertions(+), 97 deletions(-) diff --git a/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml index 84a96b8e825df..675c9142f2f6b 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/localization/ekf_localizer/README.md b/localization/ekf_localizer/README.md index 5f02f35bab1a0..baa770d7dcf9f 100644 --- a/localization/ekf_localizer/README.md +++ b/localization/ekf_localizer/README.md @@ -1,6 +1,6 @@ # Overview -The **Extend Kalman Filter Localizer** estimates robust and less noisy robot pose and twist by integrating the 2D vehicle dynamics model with input ego-pose and ego-twist messages. The algorithm is designed especially for fast moving robot such as autonomous driving system. +The **Extend Kalman Filter Localizer** estimates robust and less noisy robot pose and twist by integrating the 2D vehicle dynamics model with input ego-pose and ego-twist messages. The algorithm is designed especially for fast-moving robots such as autonomous driving systems. ## Flowchart @@ -14,10 +14,10 @@ The overall flowchart of the ekf_localizer is described below. This package includes the following features: -- **Time delay compensation** for input messages, which enables proper integration of input information with varying time delay. This is important especially for high speed moving robot, such as autonomous driving vehicle. (see following figure). +- **Time delay compensation** for input messages, which enables proper integration of input information with varying time delays. This is important especially for high-speed moving robots, such as autonomous driving vehicles. (see the following figure). - **Automatic estimation of yaw bias** prevents modeling errors caused by sensor mounting angle errors, which can improve estimation accuracy. - **Mahalanobis distance gate** enables probabilistic outlier detection to determine which inputs should be used or ignored. -- **Smooth update**, the Kalman Filter measurement update is typically performed when a measurement is obtained, but it can cause large changes in the estimated value especially for low frequency measurements. Since the algorithm can consider the measurement time, the measurement data can be divided into multiple pieces and integrated smoothly while maintaining consistency (see following figure). +- **Smooth update**, the Kalman Filter measurement update is typically performed when a measurement is obtained, but it can cause large changes in the estimated value, especially for low-frequency measurements. Since the algorithm can consider the measurement time, the measurement data can be divided into multiple pieces and integrated smoothly while maintaining consistency (see the following figure).

@@ -43,15 +43,15 @@ The parameters and input topic names can be set in the `ekf_localizer.launch` fi - measured_pose_with_covariance (geometry_msgs/PoseWithCovarianceStamped) - Input pose source with measurement covariance matrix. + Input pose source with the measurement covariance matrix. - measured_twist_with_covariance (geometry_msgs/TwistWithCovarianceStamped) - Input twist source with measurement covariance matrix. + Input twist source with the measurement covariance matrix. - initialpose (geometry_msgs/PoseWithCovarianceStamped) - Initial pose for EKF. The estimated pose is initialized with zeros at start. It is initialized with this message whenever published. + Initial pose for EKF. The estimated pose is initialized with zeros at the start. It is initialized with this message whenever published. ### Published Topics @@ -81,7 +81,7 @@ The parameters and input topic names can be set in the `ekf_localizer.launch` fi - ekf_twist_with_covariance (geometry_msgs/TwistWithCovarianceStamped) - Estimated twist with covariance. + The estimated twist with covariance. ### Published TF @@ -93,13 +93,13 @@ The parameters and input topic names can be set in the `ekf_localizer.launch` fi ### Predict -The current robot state is predicted from previously estimated data using a given prediction model. This calculation is called at constant interval (`predict_frequency [Hz]`). The prediction equation is described at the end of this page. +The current robot state is predicted from previously estimated data using a given prediction model. This calculation is called at a constant interval (`predict_frequency [Hz]`). The prediction equation is described at the end of this page. ### Measurement Update -Before update, the Mahalanobis distance is calculated between the measured input and the predicted state, the measurement update is not performed for inputs where the Mahalanobis distance exceeds the given threshold. +Before the update, the Mahalanobis distance is calculated between the measured input and the predicted state, the measurement update is not performed for inputs where the Mahalanobis distance exceeds the given threshold. -The predicted state is updated with the latest measured inputs, measured_pose and measured_twist. The updates are performed with the same frequency as prediction, usually at a high frequency, in order to enable smooth state estimation. +The predicted state is updated with the latest measured inputs, measured_pose, and measured_twist. The updates are performed with the same frequency as prediction, usually at a high frequency, in order to enable smooth state estimation. ## Parameter description @@ -117,20 +117,20 @@ The parameters are set in `launch/ekf_localizer.launch` . ### For pose measurement -| Name | Type | Description | Default value | -| :---------------------------- | :----- | :---------------------------------------------------------------- | :------------ | -| pose_additional_delay | double | Additional delay time for pose measurement [s] | 0.0 | -| pose_measure_uncertainty_time | double | Measured time uncertainty used for covariance calculation [s] | 0.01 | -| pose_rate | double | Approximated input pose rate used for covariance calculation [Hz] | 10.0 | -| pose_gate_dist | double | Limit of Mahalanobis distance used for outliers detection | 10000.0 | +| Name | Type | Description | Default value | +| :---------------------------- | :----- | :------------------------------------------------------------ | :------------ | +| pose_additional_delay | double | Additional delay time for pose measurement [s] | 0.0 | +| pose_measure_uncertainty_time | double | Measured time uncertainty used for covariance calculation [s] | 0.01 | +| pose_smoothing_steps | int | A value for smoothing steps | 5 | +| pose_gate_dist | double | Limit of Mahalanobis distance used for outliers detection | 10000.0 | ### For twist measurement -| Name | Type | Description | Default value | -| :--------------------- | :----- | :----------------------------------------------------------------- | :------------ | -| twist_additional_delay | double | Additional delay time for twist [s] | 0.0 | -| twist_rate | double | Approximated input twist rate used for covariance calculation [Hz] | 10.0 | -| twist_gate_dist | double | Limit of Mahalanobis distance used for outliers detection | 10000.0 | +| Name | Type | Description | Default value | +| :--------------------- | :----- | :-------------------------------------------------------- | :------------ | +| twist_additional_delay | double | Additional delay time for twist [s] | 0.0 | +| twist_smoothing_steps | int | A value for smoothing steps | 2 | +| twist_gate_dist | double | Limit of Mahalanobis distance used for outliers detection | 10000.0 | ### For process noise @@ -141,28 +141,30 @@ The parameters are set in `launch/ekf_localizer.launch` . | proc_stddev_yaw_c | double | Standard deviation of process noise in time differentiation expression of yaw, noise for d_yaw = omega | 0.005 | | proc_stddev_yaw_bias_c | double | Standard deviation of process noise in time differentiation expression of yaw_bias, noise for d_yaw_bias = 0 | 0.001 | -note: process noise for position x & y are calculated automatically from nonlinear dynamics. +note: process noise for positions x & y are calculated automatically from nonlinear dynamics. -## How to turn EKF parameters +## How to tune EKF parameters ### 0. Preliminaries -- Check header time in pose and twist message is set to sensor time appropriately, because time delay is calculated from this value. If it is difficult to set appropriate time due to timer synchronization problem, use `twist_additional_delay` and `pose_additional_delay` to correct the time. -- Check the relation between measurement pose and twist is appropriate (whether the derivative of pose has similar value to twist). This discrepancy is caused mainly by unit error (such as confusing radian/degree) or bias noise, and it causes large estimation errors. +- Check header time in pose and twist message is set to sensor time appropriately, because time delay is calculated from this value. If it is difficult to set an appropriate time due to the timer synchronization problem, use `twist_additional_delay` and `pose_additional_delay` to correct the time. +- Check if the relation between measurement pose and twist is appropriate (whether the derivative of the pose has a similar value to twist). This discrepancy is caused mainly by unit error (such as confusing radian/degree) or bias noise, and it causes large estimation errors. -### 1. Set sensor parameters +### 1. Tune sensor parameters -Set sensor-rate and standard-deviation from the basic information of the sensor. The `pose_measure_uncertainty_time` is for uncertainty of the header timestamp data. +Set standard deviation for each sensor. The `pose_measure_uncertainty_time` is for the uncertainty of the header timestamp data. +You can also tune a number of steps for smoothing for each observed sensor data by tuning `*_smoothing_steps`. +Increasing the number will improve the smoothness of the estimation, but may have an adverse effect on the estimation performance. - `pose_measure_uncertainty_time` -- `pose_rate` -- `twist_rate` +- `pose_smoothing_steps` +- `twist_smoothing_steps` -### 2. Set process model parameters +### 2. Tune process model parameters - `proc_stddev_vx_c` : set to maximum linear acceleration - `proc_stddev_wz_c` : set to maximum angular acceleration -- `proc_stddev_yaw_c` : This parameter describes the correlation between the yaw and yaw-rate. Large value means the change in yaw does not correlate to the estimated yaw-rate. If this is set to 0, it means the change in estimate yaw is equal to yaw-rate. Usually this should be set to 0. +- `proc_stddev_yaw_c` : This parameter describes the correlation between the yaw and yawrate. A large value means the change in yaw does not correlate to the estimated yawrate. If this is set to 0, it means the change in estimated yaw is equal to yawrate. Usually, this should be set to 0. - `proc_stddev_yaw_bias_c` : This parameter is the standard deviation for the rate of change in yaw bias. In most cases, yaw bias is constant, so it can be very small, but must be non-zero. ## Kalman Filter Model @@ -171,15 +173,15 @@ Set sensor-rate and standard-deviation from the basic information of the sensor. -where `b_k` is the yaw-bias. +where `b_k` is the yawbias. ### time delay model -The measurement time delay is handled by an augmented states [1] (See, Section 7.3 FIXED-LAG SMOOTHING). +The measurement time delay is handled by an augmented state [1] (See, Section 7.3 FIXED-LAG SMOOTHING). -Note that, although the dimension gets larger, since the analytical expansion can be applied based on the specific structures of the augmented states, the computational complexity does not significantly change. +Note that, although the dimension gets larger since the analytical expansion can be applied based on the specific structures of the augmented states, the computational complexity does not significantly change. ## Test Result with Autoware NDT @@ -187,6 +189,10 @@ Note that, although the dimension gets larger, since the analytical expansion ca

+## Known issues + +- In the presence of multiple inputs with yaw estimation, yaw bias `b_k` in the current EKF state would not make any sense, since it is intended to capture the extrinsic parameter's calibration error of a sensor. Thus, future work includes introducing yaw bias for each sensor with yaw estimation. + ## reference [1] Anderson, B. D. O., & Moore, J. B. (1979). Optimal filtering. Englewood Cliffs, NJ: Prentice-Hall. diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index c813369c67b77..67ed08b8c5962 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -39,9 +39,74 @@ #include #include #include +#include #include #include +struct PoseInfo +{ + geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr pose; + int counter; + int smoothing_steps; +}; + +struct TwistInfo +{ + geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr twist; + int counter; + int smoothing_steps; +}; + +class Simple1DFilter +{ +public: + Simple1DFilter() + { + initialized_ = false; + x_ = 0; + stddev_ = 1e9; + proc_stddev_x_c_ = 0.0; + return; + }; + void init(const double init_obs, const double obs_stddev, const rclcpp::Time time) + { + x_ = init_obs; + stddev_ = obs_stddev; + latest_time_ = time; + initialized_ = true; + return; + }; + void update(const double obs, const double obs_stddev, const rclcpp::Time time) + { + if (!initialized_) { + init(obs, obs_stddev, 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); + + // Update step + double kalman_gain = stddev_ * stddev_ / (stddev_ * stddev_ + obs_stddev * obs_stddev); + x_ = x_ + kalman_gain * (obs - x_); + stddev_ = std::sqrt(1 - kalman_gain) * stddev_; + + latest_time_ = time; + return; + }; + void set_proc_stddev(const double proc_stddev) { proc_stddev_x_c_ = proc_stddev; } + double get_x() { return x_; } + +private: + bool initialized_; + double x_; + double stddev_; + double proc_stddev_x_c_; + rclcpp::Time latest_time_; +}; + class EKFLocalizer : public rclcpp::Node { public: @@ -87,6 +152,9 @@ class EKFLocalizer : public rclcpp::Node std::shared_ptr tf_br_; //!< @brief extended kalman filter instance. TimeDelayKalmanFilter ekf_; + Simple1DFilter z_filter_; + Simple1DFilter roll_filter_; + Simple1DFilter pitch_filter_; /* parameters */ bool show_debug_info_; @@ -138,16 +206,18 @@ class EKFLocalizer : public rclcpp::Node }; /* for model prediction */ - geometry_msgs::msg::TwistStamped::SharedPtr - current_twist_ptr_; //!< @brief current measured twist - geometry_msgs::msg::PoseStamped::SharedPtr current_pose_ptr_; //!< @brief current measured pose - geometry_msgs::msg::PoseStamped current_ekf_pose_; //!< @brief current estimated pose + std::queue current_twist_info_queue_; //!< @brief current measured pose + std::queue current_pose_info_queue_; //!< @brief current measured pose + geometry_msgs::msg::PoseStamped current_ekf_pose_; //!< @brief current estimated pose geometry_msgs::msg::PoseStamped current_ekf_pose_no_yawbias_; //!< @brief current estimated pose w/o yaw bias geometry_msgs::msg::TwistStamped current_ekf_twist_; //!< @brief current estimated twist std::array current_pose_covariance_; std::array current_twist_covariance_; + int pose_smoothing_steps_; + int twist_smoothing_steps_; + /** * @brief computes update & prediction of EKF for each ekf_dt_[s] time */ @@ -192,13 +262,13 @@ class EKFLocalizer : public rclcpp::Node * @brief compute EKF update with pose measurement * @param pose measurement value */ - void measurementUpdatePose(const geometry_msgs::msg::PoseStamped & pose); + void measurementUpdatePose(const geometry_msgs::msg::PoseWithCovarianceStamped & pose); /** * @brief compute EKF update with pose measurement * @param twist measurement value */ - void measurementUpdateTwist(const geometry_msgs::msg::TwistStamped & twist); + void measurementUpdateTwist(const geometry_msgs::msg::TwistWithCovarianceStamped & twist); /** * @brief check whether a measurement value falls within the mahalanobis distance threshold @@ -241,6 +311,8 @@ class EKFLocalizer : public rclcpp::Node */ void showCurrentX(); + void updateSimple1DFilters(const geometry_msgs::msg::PoseWithCovarianceStamped & pose); + tier4_autoware_utils::StopWatch stop_watch_; friend class EKFLocalizerTestSuite; // for test code diff --git a/localization/ekf_localizer/launch/ekf_localizer.launch.xml b/localization/ekf_localizer/launch/ekf_localizer.launch.xml index cfaa37a2f6cf4..1fc07564a5d73 100644 --- a/localization/ekf_localizer/launch/ekf_localizer.launch.xml +++ b/localization/ekf_localizer/launch/ekf_localizer.launch.xml @@ -11,13 +11,13 @@ - + - + @@ -53,11 +53,11 @@ - + - + diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 6f645b7a83ce6..93b2ccd18654c 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -46,13 +47,13 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti /* pose measurement */ pose_additional_delay_ = declare_parameter("pose_additional_delay", 0.0); pose_measure_uncertainty_time_ = declare_parameter("pose_measure_uncertainty_time", 0.01); - pose_rate_ = declare_parameter("pose_rate", 10.0); // used for covariance calculation pose_gate_dist_ = declare_parameter("pose_gate_dist", 10000.0); // Mahalanobis limit + pose_smoothing_steps_ = declare_parameter("pose_smoothing_steps", 5); /* twist measurement */ twist_additional_delay_ = declare_parameter("twist_additional_delay", 0.0); - twist_rate_ = declare_parameter("twist_rate", 10.0); // used for covariance calculation twist_gate_dist_ = declare_parameter("twist_gate_dist", 10000.0); // Mahalanobis limit + twist_smoothing_steps_ = declare_parameter("twist_smoothing_steps", 2); /* process noise */ proc_stddev_yaw_c_ = declare_parameter("proc_stddev_yaw_c", 0.005); @@ -105,6 +106,10 @@ 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); + /* debug */ pub_debug_ = create_publisher("debug", 1); pub_measured_pose_ = create_publisher("debug/measured_pose", 1); @@ -151,21 +156,41 @@ void EKFLocalizer::timerCallback() DEBUG_INFO(get_logger(), "------------------------- end prediction -------------------------\n"); /* pose measurement update */ - if (current_pose_ptr_ != nullptr) { + if (!current_pose_info_queue_.empty()) { DEBUG_INFO(get_logger(), "------------------------- start Pose -------------------------"); stop_watch_.tic(); - measurementUpdatePose(*current_pose_ptr_); + + int pose_info_queue_size = static_cast(current_pose_info_queue_.size()); + for (int i = 0; i < pose_info_queue_size; ++i) { + PoseInfo pose_info = current_pose_info_queue_.front(); + current_pose_info_queue_.pop(); + measurementUpdatePose(*pose_info.pose); + ++pose_info.counter; + if (pose_info.counter < pose_info.smoothing_steps) { + current_pose_info_queue_.push(pose_info); + } + } DEBUG_INFO(get_logger(), "[EKF] measurementUpdatePose calc time = %f [ms]", stop_watch_.toc()); DEBUG_INFO(get_logger(), "------------------------- end Pose -------------------------\n"); } /* twist measurement update */ - if (current_twist_ptr_ != nullptr) { - DEBUG_INFO(get_logger(), "------------------------- start twist -------------------------"); + if (!current_twist_info_queue_.empty()) { + DEBUG_INFO(get_logger(), "------------------------- start Twist -------------------------"); stop_watch_.tic(); - measurementUpdateTwist(*current_twist_ptr_); + + int twist_info_queue_size = static_cast(current_twist_info_queue_.size()); + for (int i = 0; i < twist_info_queue_size; ++i) { + TwistInfo twist_info = current_twist_info_queue_.front(); + current_twist_info_queue_.pop(); + measurementUpdateTwist(*twist_info.twist); + ++twist_info.counter; + if (twist_info.counter < twist_info.smoothing_steps) { + current_twist_info_queue_.push(twist_info); + } + } DEBUG_INFO(get_logger(), "[EKF] measurementUpdateTwist calc time = %f [ms]", stop_watch_.toc()); - DEBUG_INFO(get_logger(), "------------------------- end twist -------------------------\n"); + DEBUG_INFO(get_logger(), "------------------------- end Twist -------------------------\n"); } /* set current pose, twist */ @@ -193,15 +218,9 @@ void EKFLocalizer::setCurrentResult() current_ekf_pose_.header.stamp = this->now(); current_ekf_pose_.pose.position.x = ekf_.getXelement(IDX::X); current_ekf_pose_.pose.position.y = ekf_.getXelement(IDX::Y); - - tf2::Quaternion q_tf; - double roll = 0.0, pitch = 0.0; - if (current_pose_ptr_ != nullptr) { - current_ekf_pose_.pose.position.z = current_pose_ptr_->pose.position.z; - tf2::fromMsg(current_pose_ptr_->pose.orientation, q_tf); /* use Pose pitch and roll */ - double yaw_tmp; - tf2::Matrix3x3(q_tf).getRPY(roll, pitch, yaw_tmp); - } + current_ekf_pose_.pose.position.z = z_filter_.get_x(); + double roll = roll_filter_.get_x(); + double pitch = pitch_filter_.get_x(); double yaw = ekf_.getXelement(IDX::YAW) + ekf_.getXelement(IDX::YAWB); current_ekf_pose_.pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(roll, pitch, yaw); @@ -307,7 +326,9 @@ void EKFLocalizer::callbackInitialPose( ekf_.init(X, P, extend_state_step_); - current_pose_ptr_ = nullptr; + updateSimple1DFilters(*initialpose); + + while (!current_pose_info_queue_.empty()) current_pose_info_queue_.pop(); } /* @@ -316,11 +337,10 @@ void EKFLocalizer::callbackInitialPose( void EKFLocalizer::callbackPoseWithCovariance( geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { - geometry_msgs::msg::PoseStamped pose; - pose.header = msg->header; - pose.pose = msg->pose.pose; - current_pose_ptr_ = std::make_shared(pose); - current_pose_covariance_ = msg->pose.covariance; + PoseInfo pose_info = {msg, 0, pose_smoothing_steps_}; + current_pose_info_queue_.push(pose_info); + + updateSimple1DFilters(*msg); } /* @@ -329,11 +349,8 @@ void EKFLocalizer::callbackPoseWithCovariance( void EKFLocalizer::callbackTwistWithCovariance( geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { - geometry_msgs::msg::TwistStamped twist; - twist.header = msg->header; - twist.twist = msg->twist.twist; - current_twist_ptr_ = std::make_shared(twist); - current_twist_covariance_ = msg->twist.covariance; + TwistInfo twist_info = {msg, 0, twist_smoothing_steps_}; + current_twist_info_queue_.push(twist_info); } /* @@ -433,7 +450,7 @@ void EKFLocalizer::predictKinematicsModel() /* * measurementUpdatePose */ -void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseStamped & pose) +void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) { if (pose.header.frame_id != pose_frame_id_) { RCLCPP_WARN_THROTTLE( @@ -468,14 +485,14 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseStamped & DEBUG_INFO(get_logger(), "delay_time: %f [s]", delay_time); /* Set yaw */ - double yaw = tf2::getYaw(pose.pose.orientation); + double yaw = tf2::getYaw(pose.pose.pose.orientation); const double ekf_yaw = ekf_.getXelement(delay_step * dim_x_ + IDX::YAW); const double yaw_error = normalizeYaw(yaw - ekf_yaw); // normalize the error not to exceed 2 pi yaw = yaw_error + ekf_yaw; /* Set measurement matrix */ Eigen::MatrixXd y(dim_y, 1); - y << pose.pose.position.x, pose.pose.position.y, yaw; + y << pose.pose.pose.position.x, pose.pose.pose.position.y, yaw; if (isnan(y.array()).any() || isinf(y.array()).any()) { RCLCPP_WARN( @@ -511,19 +528,20 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseStamped & /* Set measurement noise covariance */ Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - R(0, 0) = current_pose_covariance_.at(0); // x - x - R(0, 1) = current_pose_covariance_.at(1); // x - y - R(0, 2) = current_pose_covariance_.at(5); // x - yaw - R(1, 0) = current_pose_covariance_.at(6); // y - x - R(1, 1) = current_pose_covariance_.at(7); // y - y - R(1, 2) = current_pose_covariance_.at(11); // y - yaw - R(2, 0) = current_pose_covariance_.at(30); // yaw - x - R(2, 1) = current_pose_covariance_.at(31); // yaw - y - R(2, 2) = current_pose_covariance_.at(35); // yaw - yaw + std::array current_pose_covariance = pose.pose.covariance; + R(0, 0) = current_pose_covariance.at(0); // x - x + R(0, 1) = current_pose_covariance.at(1); // x - y + R(0, 2) = current_pose_covariance.at(5); // x - yaw + R(1, 0) = current_pose_covariance.at(6); // y - x + R(1, 1) = current_pose_covariance.at(7); // y - y + R(1, 2) = current_pose_covariance.at(11); // y - yaw + R(2, 0) = current_pose_covariance.at(30); // yaw - x + R(2, 1) = current_pose_covariance.at(31); // yaw - y + R(2, 2) = current_pose_covariance.at(35); // yaw - yaw /* In order to avoid a large change at the time of updating, * measurement update is performed by dividing at every step. */ - R *= (ekf_rate_ / pose_rate_); + R *= pose_smoothing_steps_; ekf_.updateWithDelay(y, C, R, delay_step); @@ -537,7 +555,8 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseStamped & /* * measurementUpdateTwist */ -void EKFLocalizer::measurementUpdateTwist(const geometry_msgs::msg::TwistStamped & twist) +void EKFLocalizer::measurementUpdateTwist( + const geometry_msgs::msg::TwistWithCovarianceStamped & twist) { if (twist.header.frame_id != "base_link") { RCLCPP_WARN_THROTTLE( @@ -573,7 +592,7 @@ void EKFLocalizer::measurementUpdateTwist(const geometry_msgs::msg::TwistStamped /* Set measurement matrix */ Eigen::MatrixXd y(dim_y, 1); - y << twist.twist.linear.x, twist.twist.angular.z; + y << twist.twist.twist.linear.x, twist.twist.twist.angular.z; if (isnan(y.array()).any() || isinf(y.array()).any()) { RCLCPP_WARN( @@ -608,14 +627,15 @@ void EKFLocalizer::measurementUpdateTwist(const geometry_msgs::msg::TwistStamped /* Set measurement noise covariance */ Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - R(0, 0) = current_twist_covariance_.at(0); // vx - vx - R(0, 1) = current_twist_covariance_.at(5); // vx - wz - R(1, 0) = current_twist_covariance_.at(30); // wz - vx - R(1, 1) = current_twist_covariance_.at(35); // wz - wz + std::array current_twist_covariance = twist.twist.covariance; + R(0, 0) = current_twist_covariance.at(0); // vx - vx + R(0, 1) = current_twist_covariance.at(5); // vx - wz + R(1, 0) = current_twist_covariance.at(30); // wz - vx + R(1, 1) = current_twist_covariance.at(35); // wz - wz /* In order to avoid a large change by update, measurement update is performed * by dividing at every step. measurement update is performed by dividing at every step. */ - R *= (ekf_rate_ / twist_rate_); + R *= twist_smoothing_steps_; ekf_.updateWithDelay(y, C, R, delay_step); @@ -709,17 +729,17 @@ void EKFLocalizer::publishEstimateResult() pub_odom_->publish(odometry); /* debug measured pose */ - if (current_pose_ptr_ != nullptr) { + if (!current_pose_info_queue_.empty()) { geometry_msgs::msg::PoseStamped p; - p = *current_pose_ptr_; + p.pose = current_pose_info_queue_.back().pose->pose.pose; p.header.stamp = current_time; pub_measured_pose_->publish(p); } /* debug publish */ double pose_yaw = 0.0; - if (current_pose_ptr_ != nullptr) { - pose_yaw = tf2::getYaw(current_pose_ptr_->pose.orientation); + if (!current_pose_info_queue_.empty()) { + pose_yaw = tf2::getYaw(current_pose_info_queue_.back().pose->pose.pose.orientation); } tier4_debug_msgs::msg::Float64MultiArrayStamped msg; @@ -734,3 +754,21 @@ double EKFLocalizer::normalizeYaw(const double & yaw) const { return std::atan2(std::sin(yaw), std::cos(yaw)); } + +void EKFLocalizer::updateSimple1DFilters(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) +{ + double z = pose.pose.pose.position.z; + double roll = 0.0, pitch = 0.0, yaw_tmp = 0.0; + + tf2::Quaternion q_tf; + 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]); + + 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); +}