fix: acceleration uncertainty equation fix

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
autowarefoundation · Jan 15, 2024 · 87fecae · 87fecae
1 parent 80c8d3a
commit 87fecae
Show file tree

Hide file tree

Showing 3 changed files with 6 additions and 6 deletions.
diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp
@@ -255,8 +255,8 @@ bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const
     q_stddev_slip_rate = std::min(q_stddev_slip_rate, ekf_params_.q_stddev_slip_rate_max);
     q_stddev_slip_rate = std::max(q_stddev_slip_rate, ekf_params_.q_stddev_slip_rate_min);
   }
-  const float q_cov_x = std::pow(ekf_params_.q_stddev_acc_long * dt * dt, 2.0);
-  const float q_cov_y = std::pow(ekf_params_.q_stddev_acc_lat * dt * dt, 2.0);
+  const float q_cov_x = std::pow(0.5 * ekf_params_.q_stddev_acc_long * dt * dt, 2.0);
+  const float q_cov_y = std::pow(0.5 * ekf_params_.q_stddev_acc_lat * dt * dt, 2.0);
   const float q_cov_yaw = std::pow(q_stddev_yaw_rate * dt, 2.0);
   const float q_cov_vel = std::pow(ekf_params_.q_stddev_acc_long * dt, 2.0);
   const float q_cov_slip = std::pow(q_stddev_slip_rate * dt, 2.0);

diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp
@@ -270,8 +270,8 @@ bool BigVehicleTracker::predict(const double dt, KalmanFilter & ekf) const
     q_stddev_slip_rate = std::min(q_stddev_slip_rate, ekf_params_.q_stddev_slip_rate_max);
     q_stddev_slip_rate = std::max(q_stddev_slip_rate, ekf_params_.q_stddev_slip_rate_min);
   }
-  const float q_cov_x = std::pow(ekf_params_.q_stddev_acc_long * dt * dt, 2.0);
-  const float q_cov_y = std::pow(ekf_params_.q_stddev_acc_lat * dt * dt, 2.0);
+  const float q_cov_x = std::pow(0.5 * ekf_params_.q_stddev_acc_long * dt * dt, 2.0);
+  const float q_cov_y = std::pow(0.5 * ekf_params_.q_stddev_acc_lat * dt * dt, 2.0);
   const float q_cov_yaw = std::pow(q_stddev_yaw_rate * dt, 2.0);
   const float q_cov_vel = std::pow(ekf_params_.q_stddev_acc_long * dt, 2.0);
   const float q_cov_slip = std::pow(q_stddev_slip_rate * dt, 2.0);

diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp
@@ -270,8 +270,8 @@ bool NormalVehicleTracker::predict(const double dt, KalmanFilter & ekf) const
     q_stddev_slip_rate = std::min(q_stddev_slip_rate, ekf_params_.q_stddev_slip_rate_max);
     q_stddev_slip_rate = std::max(q_stddev_slip_rate, ekf_params_.q_stddev_slip_rate_min);
   }
-  const float q_cov_x = std::pow(ekf_params_.q_stddev_acc_long * dt * dt, 2.0);
-  const float q_cov_y = std::pow(ekf_params_.q_stddev_acc_lat * dt * dt, 2.0);
+  const float q_cov_x = std::pow(0.5 * ekf_params_.q_stddev_acc_long * dt * dt, 2.0);
+  const float q_cov_y = std::pow(0.5 * ekf_params_.q_stddev_acc_lat * dt * dt, 2.0);
   const float q_cov_yaw = std::pow(q_stddev_yaw_rate * dt, 2.0);
   const float q_cov_vel = std::pow(ekf_params_.q_stddev_acc_long * dt, 2.0);
   const float q_cov_slip = std::pow(q_stddev_slip_rate * dt, 2.0);