From 08c1d2998f07cb3cb7c1a7f57c0a9b88070d4289 Mon Sep 17 00:00:00 2001 From: kminoda Date: Thu, 26 Oct 2023 14:07:23 +0900 Subject: [PATCH] update Signed-off-by: kminoda --- .../src/deviation_estimator.cpp | 15 +++++---------- 1 file changed, 5 insertions(+), 10 deletions(-) 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 43604cd6..20ae034d 100644 --- a/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator.cpp +++ b/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator.cpp @@ -316,16 +316,11 @@ void DeviationEstimator::timer_callback() stddev_vx_msg.data = stddev_vx; pub_stddev_vx_->publish(stddev_vx_msg); - // For IMU link standard deviation, we use the minimum value from the calculated 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, which is often the - // smallest value among the three axes. - double stddev_angvel_imu = std::numeric_limits::max(); - stddev_angvel_imu = std::min(stddev_angvel_imu, stddev_angvel_base.x); - stddev_angvel_imu = std::min(stddev_angvel_imu, stddev_angvel_base.y); - stddev_angvel_imu = std::min(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);