diff --git a/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator.cpp b/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator.cpp index 93e3cb4e..70874366 100644 --- a/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator.cpp +++ b/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator.cpp @@ -316,10 +316,12 @@ void DeviationEstimator::timer_callback() stddev_vx_msg.data = stddev_vx; pub_stddev_vx_->publish(stddev_vx_msg); - double stddev_angvel_imu = 0.0; - stddev_angvel_imu = std::max(stddev_angvel_imu, stddev_angvel_base.x); - stddev_angvel_imu = std::max(stddev_angvel_imu, stddev_angvel_base.y); - stddev_angvel_imu = std::max(stddev_angvel_imu, stddev_angvel_base.z); + // For IMU link standard deviation, we use the yaw standard deviation in base_link. + // This is because the standard deviation estimation of x and y in base_link may not be accurate + // especially when the data contains a motion when the people are getting on/off the vehicle, + // which causes the vehicle to tilt in roll and pitch. In this case, we would like to use the + // standard deviation of yaw axis in base_link. + double stddev_angvel_imu = stddev_angvel_base.z; geometry_msgs::msg::Vector3 stddev_angvel_imu_msg = createVector3(stddev_angvel_imu, stddev_angvel_imu, stddev_angvel_imu); pub_stddev_angvel_->publish(stddev_angvel_imu_msg);