feat(deviation_estimator): estimate gyro only when straight

Signed-off-by: kminoda <[email protected]>
tier4 · Aug 31, 2023 · 1ab9931 · 1ab9931
1 parent decf391
commit 1ab9931
Show file tree

Hide file tree

Showing 2 changed files with 58 additions and 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
@@ -255,16 +255,18 @@ 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_;
 
   if (is_straight & !is_stopped & is_constant_velocity) {
     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
@@ -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<TrajectoryData> traj_data_list;
+
+  for (int tmp = 0; tmp < n_all; ++tmp) {
+    std::vector<geometry_msgs::msg::Vector3Stamped> 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<geometry_msgs::msg::PoseStamped> 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);
+}