Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix(ekf_localizer): update predict frequency with measured timer rate #399

Merged
merged 4 commits into from
Feb 22, 2022
Merged

fix(ekf_localizer): update predict frequency with measured timer rate #399

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
605896f
fix(ekf_localizer): update predict frequency with measured timer rate
kosuke55 Feb 16, 2022
cd797c2
Remove timer smoothing
kosuke55 Feb 17, 2022
abc283b
Remove deque
kosuke55 Feb 19, 2022
ddc2f38
Remove clock_
kosuke55 Feb 19, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 14 additions & 0 deletions localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -78,6 +78,9 @@ class EKFLocalizer : public rclcpp::Node
sub_twist_with_cov_;
//!< @brief time for ekf calculation callback
rclcpp::TimerBase::SharedPtr timer_control_;
//!< @brief last predict time
std::shared_ptr<const rclcpp::Time> last_predict_time_;

//!< @brief timer to send transform
rclcpp::TimerBase::SharedPtr timer_tf_;
//!< @brief tf broadcaster
Expand Down Expand Up @@ -113,6 +116,12 @@ class EKFLocalizer : public rclcpp::Node
//!< @brief measurement is ignored if the mahalanobis distance is larger than this value.
double twist_gate_dist_;

/* process noise standard deviation */
double proc_stddev_yaw_c_; //!< @brief yaw process noise
double proc_stddev_yaw_bias_c_; //!< @brief yaw bias process noise
double proc_stddev_vx_c_; //!< @brief vx process noise
double proc_stddev_wz_c_; //!< @brief wz process noise

/* process noise variance for discrete model */
double proc_cov_yaw_d_; //!< @brief discrete yaw process noise
double proc_cov_yaw_bias_d_; //!< @brief discrete yaw bias process noise
Expand Down Expand Up @@ -169,6 +178,11 @@ class EKFLocalizer : public rclcpp::Node
*/
void initEKF();

/**
* @brief update predict frequency
*/
void updatePredictFrequency();

/**
* @brief compute EKF prediction
*/
Expand Down
41 changes: 31 additions & 10 deletions localization/ekf_localizer/src/ekf_localizer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,20 +55,19 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti
twist_gate_dist_ = declare_parameter("twist_gate_dist", 10000.0); // Mahalanobis limit

/* process noise */
double proc_stddev_yaw_c, proc_stddev_yaw_bias_c, proc_stddev_vx_c, proc_stddev_wz_c;
proc_stddev_yaw_c = declare_parameter("proc_stddev_yaw_c", 0.005);
proc_stddev_yaw_bias_c = declare_parameter("proc_stddev_yaw_bias_c", 0.001);
proc_stddev_vx_c = declare_parameter("proc_stddev_vx_c", 5.0);
proc_stddev_wz_c = declare_parameter("proc_stddev_wz_c", 1.0);
proc_stddev_yaw_c_ = declare_parameter("proc_stddev_yaw_c", 0.005);
proc_stddev_yaw_bias_c_ = declare_parameter("proc_stddev_yaw_bias_c", 0.001);
proc_stddev_vx_c_ = declare_parameter("proc_stddev_vx_c", 5.0);
proc_stddev_wz_c_ = declare_parameter("proc_stddev_wz_c", 1.0);
if (!enable_yaw_bias_estimation_) {
proc_stddev_yaw_bias_c = 0.0;
proc_stddev_yaw_bias_c_ = 0.0;
}

/* convert to continuous to discrete */
proc_cov_vx_d_ = std::pow(proc_stddev_vx_c * ekf_dt_, 2.0);
proc_cov_wz_d_ = std::pow(proc_stddev_wz_c * ekf_dt_, 2.0);
proc_cov_yaw_d_ = std::pow(proc_stddev_yaw_c * ekf_dt_, 2.0);
proc_cov_yaw_bias_d_ = std::pow(proc_stddev_yaw_bias_c * ekf_dt_, 2.0);
proc_cov_vx_d_ = std::pow(proc_stddev_vx_c_ * ekf_dt_, 2.0);
proc_cov_wz_d_ = std::pow(proc_stddev_wz_c_ * ekf_dt_, 2.0);
proc_cov_yaw_d_ = std::pow(proc_stddev_yaw_c_ * ekf_dt_, 2.0);
proc_cov_yaw_bias_d_ = std::pow(proc_stddev_yaw_bias_c_ * ekf_dt_, 2.0);

/* initialize ros system */
auto period_control_ns =
Expand Down Expand Up @@ -111,13 +110,35 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti
pub_measured_pose_ = create_publisher<geometry_msgs::msg::PoseStamped>("debug/measured_pose", 1);
}

/*
* updatePredictFrequency
*/
void EKFLocalizer::updatePredictFrequency()
{
if (last_predict_time_) {
ekf_rate_ = 1.0 / (get_clock()->now() - *last_predict_time_).seconds();
DEBUG_INFO(get_logger(), "[EKF] update ekf_rate_ to %f hz", ekf_rate_);
ekf_dt_ = 1.0 / std::max(ekf_rate_, 0.1);

/* Update discrete proc_cov*/
proc_cov_vx_d_ = std::pow(proc_stddev_vx_c_ * ekf_dt_, 2.0);
proc_cov_wz_d_ = std::pow(proc_stddev_wz_c_ * ekf_dt_, 2.0);
proc_cov_yaw_d_ = std::pow(proc_stddev_yaw_c_ * ekf_dt_, 2.0);
proc_cov_yaw_bias_d_ = std::pow(proc_stddev_yaw_bias_c_ * ekf_dt_, 2.0);
}
last_predict_time_ = std::make_shared<const rclcpp::Time>(get_clock()->now());
}

/*
* timerCallback
*/
void EKFLocalizer::timerCallback()
{
DEBUG_INFO(get_logger(), "========================= timer called =========================");

/* update predict frequency with measured timer rate */
updatePredictFrequency();

/* predict model in EKF */
stop_watch_.tic();
DEBUG_INFO(get_logger(), "------------------------- start prediction -------------------------");
Expand Down