Skip to content

fix: imu scale estimation covariance limit for stability #12562

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

Merged
SergioReyesSan merged 5 commits into from
Jun 5, 2026
+19 −16
Merged

fix: imu scale estimation covariance limit for stability #12562

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
f536089
fix: imu scale estimation covariance limit for stability
SergioReyesSan May 8, 2026
202b165
Merge branch 'main' into fix/covariance_limit
SergioReyesSan May 20, 2026
b4bee70
Merge branch 'main' into fix/covariance_limit
YoshiRi May 22, 2026
2b18f4c
chore: change covaraince limit
SergioReyesSan Jun 1, 2026
3893e64
Merge branch 'main' into fix/covariance_limit
SergioReyesSan Jun 5, 2026
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
4 changes: 2 additions & 2 deletions sensing/autoware_imu_corrector/config/gyro_scale_estimator.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@
estimate_scale_init: 1.0
min_allowed_scale: 0.9
max_allowed_scale: 1.1
threshold_to_estimate_scale: 0.1
threshold_to_estimate_scale: 0.05
percentage_scale_rate_allow_correct: 0.04
alpha: 0.99
delay_gyro_ms: 170
Expand All @@ -28,7 +28,7 @@
variance_p_angle: 5e-9
measurement_noise_r_angle: 0.00005
min_covariance_angle: 1e-11
decay_coefficient: 0.99999998
decay_coefficient: 0.9999999998
samples_in_bounds_to_init: 250
max_reinitialization_retries: 5

Expand Down
4 changes: 2 additions & 2 deletions sensing/autoware_imu_corrector/schema/gyro_scale_estimator.schema.json
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@
"threshold_to_estimate_scale": {
"type": "number",
"description": "Angular speed radians for scale estimation threshold.",
"default": 0.1
"default": 0.05
},
"percentage_scale_rate_allow_correct": {
"type": "number",
Expand Down Expand Up @@ -147,7 +147,7 @@
"decay_coefficient": {
"type": "number",
"description": "Decay coefficient for EKF.",
"default": 0.99999998
"default": 0.9999999998
},
"samples_in_bounds_to_init": {
"type": "integer",
Expand Down
27 changes: 15 additions & 12 deletions sensing/autoware_imu_corrector/src/gyro_bias_estimator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,11 @@

namespace autoware::imu_corrector
{
namespace
{
constexpr double kYawStateProcessNoise = 1e-8;
} // namespace

GyroBiasEstimator::GyroBiasEstimator(const rclcpp::NodeOptions & options)
: rclcpp::Node("gyro_bias_scale_validator", options),
gyro_bias_threshold_(declare_parameter<double>("gyro_bias_threshold")),
Expand Down Expand Up @@ -139,7 +144,8 @@
ekf_angle_.estimated_scale_angle_ = ekf_angle_.initial_scale_;
ekf_angle_.max_variance_p_angle_ = declare_parameter<double>("ekf_angle.variance_p_angle");
ekf_angle_.noise_ekf_r_angle_ = declare_parameter<double>("ekf_angle.measurement_noise_r_angle");
ekf_angle_.q_angle_ << 0, 0, 0, declare_parameter<double>("ekf_angle.process_noise_q_angle");
ekf_angle_.q_angle_ << kYawStateProcessNoise, 0, 0,
declare_parameter<double>("ekf_angle.process_noise_q_angle");

Check warning on line 148 in sensing/autoware_imu_corrector/src/gyro_bias_estimator.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Code Health Review (main)

❌ Getting worse: Large Method 

GyroBiasEstimator::GyroBiasEstimator increases from 126 to 127 lines of code, threshold = 70. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.
ekf_angle_.p_angle_ << ekf_angle_.max_variance_p_angle_, 0, 0, ekf_angle_.max_variance_p_angle_;
ekf_angle_.r_angle_ << ekf_angle_.noise_ekf_r_angle_ * ekf_angle_.noise_ekf_r_angle_;
ekf_angle_.filtered_scale_angle_ = ekf_angle_.initial_scale_;
Expand Down Expand Up @@ -271,16 +277,13 @@
Eigen::Matrix2d f_matrix;
f_matrix << 1, dt_imu * (gyro.vector.z - gyro_bias_not_rotated_.value().z), 0,
ekf_angle_.decay_coefficient_;
ekf_angle_.p_angle_ =
f_matrix * ekf_angle_.p_angle_ * f_matrix.transpose() + ekf_angle_.q_angle_;

// Limit covariance
ekf_angle_.p_angle_(0, 0) = std::min(
std::max(ekf_angle_.p_angle_(0, 0), ekf_angle_.min_covariance_angle_),
ekf_angle_.max_variance_p_angle_);
ekf_angle_.p_angle_(1, 1) = std::min(
std::max(ekf_angle_.p_angle_(1, 1), ekf_angle_.min_covariance_angle_),
ekf_angle_.max_variance_p_angle_);
// Predict covariance with F first, then skip Q injection once reach max allowed variance.
ekf_angle_.p_angle_ = f_matrix * ekf_angle_.p_angle_ * f_matrix.transpose();
if (
ekf_angle_.p_angle_(0, 0) < ekf_angle_.max_variance_p_angle_ &&
ekf_angle_.p_angle_(1, 1) < ekf_angle_.max_variance_p_angle_) {
ekf_angle_.p_angle_ += ekf_angle_.q_angle_;
}

if (gyro_yaw_angle_ < -M_PI) {
gyro_yaw_angle_ += 2.0 * M_PI;
Expand Down Expand Up @@ -341,7 +344,7 @@
ekf_angle.x_state_(1) = ekf_angle.initial_scale_; // estimated scale
ekf_angle.estimated_scale_angle_ = ekf_angle.initial_scale_;
ekf_angle.p_angle_ << ekf_angle.max_variance_p_angle_, 0, 0, ekf_angle.max_variance_p_angle_;
ekf_angle.q_angle_ << 0, 0, 0, ekf_angle.process_noise_q_angle_;
ekf_angle.q_angle_ << kYawStateProcessNoise, 0, 0, ekf_angle.process_noise_q_angle_;
ekf_angle.r_angle_ << ekf_angle.noise_ekf_r_angle_ * ekf_angle.noise_ekf_r_angle_;
ekf_angle.filtered_scale_angle_ = ekf_angle.initial_scale_;
ekf_angle.has_gyro_yaw_angle_init_ = false;
Expand Down
Loading