From 1ab993110de56d6ace0dad25e82e1f6e0d45ce31 Mon Sep 17 00:00:00 2001 From: kminoda Date: Thu, 31 Aug 2023 17:53:41 +0900 Subject: [PATCH] feat(deviation_estimator): estimate gyro only when straight Signed-off-by: kminoda --- .../src/deviation_estimator.cpp | 8 +-- .../test/test_gyro_stddev.cpp | 53 +++++++++++++++++++ 2 files changed, 58 insertions(+), 3 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 388b3c72..acb0974f 100644 --- a/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator.cpp +++ b/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator.cpp @@ -255,7 +255,7 @@ void DeviationEstimator::timer_callback() traj_data.pose_list = pose_buf_; traj_data.vx_list = extract_sub_trajectory(vx_all_, t0_rclcpp_time, t1_rclcpp_time); traj_data.gyro_list = extract_sub_trajectory(gyro_all_, t0_rclcpp_time, t1_rclcpp_time); - bool is_straight = get_mean_abs_wz(traj_data.gyro_list) > wz_threshold_; + bool is_straight = get_mean_abs_wz(traj_data.gyro_list) < wz_threshold_; bool is_stopped = get_mean_abs_vx(traj_data.vx_list) < vx_threshold_; bool is_constant_velocity = std::abs(get_mean_accel(traj_data.vx_list)) < accel_threshold_; @@ -263,8 +263,10 @@ void DeviationEstimator::timer_callback() vel_coef_module_->update_coef(traj_data); traj_data_list_for_velocity_.push_back(traj_data); } - gyro_bias_module_->update_bias(traj_data); - traj_data_list_for_gyro_.push_back(traj_data); + if (is_straight) { + gyro_bias_module_->update_bias(traj_data); + traj_data_list_for_gyro_.push_back(traj_data); + } pose_buf_.clear(); diff --git a/localization/deviation_estimation_tools/deviation_estimator/test/test_gyro_stddev.cpp b/localization/deviation_estimation_tools/deviation_estimator/test/test_gyro_stddev.cpp index 0e40c27f..ac416317 100644 --- a/localization/deviation_estimation_tools/deviation_estimator/test/test_gyro_stddev.cpp +++ b/localization/deviation_estimation_tools/deviation_estimator/test/test_gyro_stddev.cpp @@ -121,3 +121,56 @@ TEST(DeviationEstimatorGyroStddev, SmokeTestWithBias) EXPECT_NEAR(estimated_gyro_stddev.y, stddev_gyro, stddev_gyro * ERROR_RATE); EXPECT_NEAR(estimated_gyro_stddev.z, stddev_gyro, stddev_gyro * ERROR_RATE); } + +TEST(DeviationEstimatorGyroStddev, SmokeTestWithBiasWithAngularVelocity) +{ + const double ERROR_RATE = 0.1; + + const int n_all = 10000; + const rclcpp::Time t_start = rclcpp::Time(0, 0); + const double stddev_gyro = 0.03141592; + const int gyro_rate = 30; + const int ndt_rate = 10; + const double t_window = 5; + const geometry_msgs::msg::Vector3 gyro_bias = createVector3(0.005, 0.001, -0.01); + const double angular_velocity = 0.2; + + std::mt19937 engine; + engine.seed(); + std::normal_distribution<> dist(angular_velocity, stddev_gyro); + + std::vector traj_data_list; + + for (int tmp = 0; tmp < n_all; ++tmp) { + std::vector gyro_data_while_stopped; + for (int i = 0; i <= gyro_rate * t_window; ++i) { + geometry_msgs::msg::Vector3Stamped gyro; + gyro.header.stamp = t_start + rclcpp::Duration::from_seconds(1.0 * i / gyro_rate); + gyro.vector.x = dist(engine) + gyro_bias.x; + gyro.vector.y = dist(engine) + gyro_bias.y; + gyro.vector.z = dist(engine) + gyro_bias.z; + gyro_data_while_stopped.push_back(gyro); + } + + std::vector pose_list; + for (int i = 0; i <= ndt_rate * t_window; ++i) { + geometry_msgs::msg::PoseStamped pose; + const auto duration = rclcpp::Duration::from_seconds(1.0 * i / ndt_rate); + pose.header.stamp = t_start + duration; + pose.pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0.0, 0.0, duration.seconds() * angular_velocity); + pose_list.push_back(pose); + } + + TrajectoryData traj_data; + traj_data.pose_list = pose_list; + traj_data.gyro_list = gyro_data_while_stopped; + traj_data_list.push_back(traj_data); + } + + geometry_msgs::msg::Vector3 estimated_gyro_stddev = + estimate_stddev_angular_velocity(traj_data_list, gyro_bias); + + EXPECT_NEAR(estimated_gyro_stddev.x, stddev_gyro, stddev_gyro * ERROR_RATE); + EXPECT_NEAR(estimated_gyro_stddev.y, stddev_gyro, stddev_gyro * ERROR_RATE); + EXPECT_NEAR(estimated_gyro_stddev.z, stddev_gyro, stddev_gyro * ERROR_RATE); +}