From 87fecaee5b0f869967dcfd4039e73e26dad78a6d Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 12 Jan 2024 18:48:32 +0900 Subject: [PATCH] fix: acceleration uncertainty equation fix Signed-off-by: Taekjin LEE --- .../src/tracker/model/bicycle_tracker.cpp | 4 ++-- .../src/tracker/model/big_vehicle_tracker.cpp | 4 ++-- .../src/tracker/model/normal_vehicle_tracker.cpp | 4 ++-- 3 files changed, 6 insertions(+), 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 index 84f33ceaa8819..30701a70a58b9 100644 --- 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 index 104895a27e754..89157fc7fc13d 100644 --- 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 index 7907898a97279..aa74b1c435306 100644 --- 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);