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): lowpass filter initialization #2402

Merged
merged 4 commits into from
Dec 1, 2022
Merged

fix(ekf_localizer): lowpass filter initialization #2402

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
3bd1386
fix(ekf_localizer): lowpass filter initialization
kminoda Nov 29, 2022
d23fd47
ci(pre-commit): autofix
pre-commit-ci[bot] Nov 30, 2022
b6749b7
debug
kminoda Nov 30, 2022
668c714
ci(pre-commit): autofix
pre-commit-ci[bot] Nov 30, 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
5 changes: 5 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 @@ -270,6 +270,11 @@ class EKFLocalizer : public rclcpp::Node
*/
void updateSimple1DFilters(const geometry_msgs::msg::PoseWithCovarianceStamped & pose);

/**
* @brief initialize simple1DFilter
*/
void initSimple1DFilters(const geometry_msgs::msg::PoseWithCovarianceStamped & pose);

/**
* @brief trigger node
*/
Expand Down
21 changes: 20 additions & 1 deletion localization/ekf_localizer/src/ekf_localizer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -334,7 +334,7 @@ void EKFLocalizer::callbackInitialPose(

ekf_.init(X, P, params_.extend_state_step);

updateSimple1DFilters(*initialpose);
initSimple1DFilters(*initialpose);
}

/*
Expand Down Expand Up @@ -681,6 +681,25 @@ void EKFLocalizer::updateSimple1DFilters(const geometry_msgs::msg::PoseWithCovar
pitch_filter_.update(pitch, pitch_dev, pose.header.stamp);
}

void EKFLocalizer::initSimple1DFilters(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);

using COV_IDX = tier4_autoware_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];

z_filter_.init(z, z_dev, pose.header.stamp);
roll_filter_.init(roll, roll_dev, pose.header.stamp);
pitch_filter_.init(pitch, pitch_dev, pose.header.stamp);
}

/**
* @brief trigger node
*/
Expand Down