diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp index 42795805f803e..47c1014062714 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.cpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp @@ -83,6 +83,12 @@ void GyroBiasEstimator::update_diagnostics(diagnostic_updater::DiagnosticStatusW stat.add("gyro_bias", "Bias estimation is not yet ready because of insufficient data."); stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Not initialized"); } else { + stat.add("gyro_bias_x_for_imu_corrector", gyro_bias_.value().x); + stat.add("gyro_bias_y_for_imu_corrector", gyro_bias_.value().y); + stat.add("gyro_bias_z_for_imu_corrector", gyro_bias_.value().z); + stat.add("estimated_gyro_bias_x", gyro_bias_.value().x - angular_velocity_offset_x_); + stat.add("estimated_gyro_bias_y", gyro_bias_.value().y - angular_velocity_offset_y_); + stat.add("estimated_gyro_bias_z", gyro_bias_.value().z - angular_velocity_offset_z_); // Validation const bool is_bias_small_enough = std::abs(gyro_bias_.value().x - angular_velocity_offset_x_) < gyro_bias_threshold_ &&