Skip to content

Commit

Permalink
refactor(ekf_localizer): rename dev to var in the Simple1DFilter class (
Browse files Browse the repository at this point in the history 
autowarefoundation#8576)

rename dev to var

Signed-off-by: Yamato Ando <[email protected]>
  • Loading branch information
@YamatoAndo
YamatoAndo authored Aug 23, 2024
1 parent a079c20 commit 23e13fd
Show file tree
Hide file tree
Showing 2 changed files with 33 additions and 33 deletions.
30 changes: 15 additions & 15 deletions localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,44 +56,44 @@ class Simple1DFilter
{
initialized_ = false;
x_ = 0;
dev_ = 1e9;
proc_dev_x_c_ = 0.0;
var_ = 1e9;
proc_var_x_c_ = 0.0;
};
void init(const double init_obs, const double obs_dev, const rclcpp::Time & time)
void init(const double init_obs, const double obs_var, const rclcpp::Time & time)
{
x_ = init_obs;
dev_ = obs_dev;
var_ = obs_var;
latest_time_ = time;
initialized_ = true;
};
void update(const double obs, const double obs_dev, const rclcpp::Time & time)
void update(const double obs, const double obs_var, const rclcpp::Time & time)
{
if (!initialized_) {
init(obs, obs_dev, time);
init(obs, obs_var, time);
return;
}

// Prediction step (current stddev_)
// Prediction step (current variance)
double dt = (time - latest_time_).seconds();
double proc_dev_x_d = proc_dev_x_c_ * dt * dt;
dev_ = dev_ + proc_dev_x_d;
double proc_var_x_d = proc_var_x_c_ * dt * dt;
var_ = var_ + proc_var_x_d;

// Update step
double kalman_gain = dev_ / (dev_ + obs_dev);
double kalman_gain = var_ / (var_ + obs_var);
x_ = x_ + kalman_gain * (obs - x_);
dev_ = (1 - kalman_gain) * dev_;
var_ = (1 - kalman_gain) * var_;

latest_time_ = time;
};
void set_proc_dev(const double proc_dev) { proc_dev_x_c_ = proc_dev; }
void set_proc_var(const double proc_var) { proc_var_x_c_ = proc_var; }
[[nodiscard]] double get_x() const { return x_; }
[[nodiscard]] double get_dev() const { return dev_; }
[[nodiscard]] double get_var() const { return var_; }

private:
bool initialized_;
double x_;
double dev_;
double proc_dev_x_c_;
double var_;
double proc_var_x_c_;
rclcpp::Time latest_time_;
};

Expand Down
36 changes: 18 additions & 18 deletions localization/ekf_localizer/src/ekf_localizer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -100,9 +100,9 @@ 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_dev(params_.z_filter_proc_dev);
roll_filter_.set_proc_dev(params_.roll_filter_proc_dev);
pitch_filter_.set_proc_dev(params_.pitch_filter_proc_dev);
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 @@ -367,9 +367,9 @@ void EKFLocalizer::publish_estimate_result(
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_dev();
pose_cov.pose.covariance[COV_IDX::ROLL_ROLL] = roll_filter_.get_dev();
pose_cov.pose.covariance[COV_IDX::PITCH_PITCH] = pitch_filter_.get_dev();
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);

Expand Down Expand Up @@ -466,14 +466,14 @@ void EKFLocalizer::update_simple_1d_filters(
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_dev = pose.pose.covariance[COV_IDX::Z_Z] * static_cast<double>(smoothing_step);
double roll_dev = pose.pose.covariance[COV_IDX::ROLL_ROLL] * static_cast<double>(smoothing_step);
double pitch_dev =
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_dev, pose.header.stamp);
roll_filter_.update(rpy.x, roll_dev, pose.header.stamp);
pitch_filter_.update(rpy.y, pitch_dev, pose.header.stamp);
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);
}

void EKFLocalizer::init_simple_1d_filters(
Expand All @@ -484,13 +484,13 @@ void EKFLocalizer::init_simple_1d_filters(
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_dev = pose.pose.covariance[COV_IDX::Z_Z];
double roll_dev = pose.pose.covariance[COV_IDX::ROLL_ROLL];
double pitch_dev = pose.pose.covariance[COV_IDX::PITCH_PITCH];
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_dev, pose.header.stamp);
roll_filter_.init(rpy.x, roll_dev, pose.header.stamp);
pitch_filter_.init(rpy.y, pitch_dev, pose.header.stamp);
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);
}

/**
Expand Down

0 comments on commit 23e13fd

Please sign in to comment.