Skip to content

Commit

Permalink
refactor(ekf_localizer): moved Simple1DFilter from ekf_localizer to e…
Browse files Browse the repository at this point in the history 
…kf_module (autowarefoundation#8705)

* Moved Simple1DFilter from ekf_localizer to ekf_module

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* feat(map_loader): visualize BusStopArea

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

* feat(map_loader): visualize bicycle_lane

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

* add maitainer

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

---------

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
Co-authored-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
Co-authored-by: Mamoru Sobue <hilo.soblin@gmail.com>
  • Loading branch information
@SakodaShintaro @soblin
3 people authored Sep 12, 2024
1 parent eaacae4 commit 19894ae
Show file tree
Hide file tree
Showing 4 changed files with 123 additions and 138 deletions.
62 changes: 0 additions & 62 deletions localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down Expand Up @@ -156,9 +113,6 @@ class EKFLocalizer : public rclcpp::Node

//!< @brief extended kalman filter instance.
std::unique_ptr<EKFModule> ekf_module_;
Simple1DFilter z_filter_;
Simple1DFilter roll_filter_;
Simple1DFilter pitch_filter_;

const HyperParameters params_;

Expand Down Expand Up @@ -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
*/
Expand All @@ -250,11 +193,6 @@ class EKFLocalizer : public rclcpp::Node

autoware::universe_utils::StopWatch<std::chrono::milliseconds> stop_watch_;

/**
* @brief last angular velocity for compensating rph with delay
*/
tf2::Vector3 last_angular_velocity_;

friend class EKFLocalizerTestSuite; // for test code
};

Expand Down
60 changes: 58 additions & 2 deletions localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand All @@ -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;
Expand All @@ -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> warning_;
const int dim_x_;
std::vector<double> 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
Expand Down
74 changes: 4 additions & 70 deletions localization/ekf_localizer/src/ekf_localizer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand Up @@ -99,10 +98,6 @@ EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options)

ekf_module_ = std::make_unique<EKFModule>(warning_, params_);
logger_configure_ = std::make_unique<autoware::universe_utils::LoggerLevelConfigure>(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);
}

/*
Expand Down Expand Up @@ -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(
Expand Down Expand Up @@ -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(
Expand All @@ -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);

Expand All @@ -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);
}
Expand Down Expand Up @@ -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);
}

/*
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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<double>(smoothing_step);
double roll_var = pose.pose.covariance[COV_IDX::ROLL_ROLL] * static_cast<double>(smoothing_step);
double pitch_var =
pose.pose.covariance[COV_IDX::PITCH_PITCH] * static_cast<double>(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
*/
Expand Down
Loading

0 comments on commit 19894ae

Please sign in to comment.