From 3f20c9882354fcb415bdbf79d6e3b87c18ed66b1 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 20 Jun 2024 11:54:45 +0900 Subject: [PATCH] fix: mis-implementation of process noise Signed-off-by: Taekjin LEE --- .../motion_model/bicycle_motion_model.cpp | 4 ++-- .../motion_model/ctrv_motion_model.cpp | 21 ++++++++++--------- .../tracker/motion_model/cv_motion_model.cpp | 8 +++---- 3 files changed, 17 insertions(+), 16 deletions(-) diff --git a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp index e3c3231c2ed70..466108a2bef0e 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp @@ -390,8 +390,8 @@ bool BicycleMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) c } const double dt2 = dt * dt; const double dt4 = dt2 * dt2; - const double q_cov_x = 0.5 * motion_params_.q_cov_acc_long * dt4; - const double q_cov_y = 0.5 * motion_params_.q_cov_acc_lat * dt4; + const double q_cov_x = 0.25 * motion_params_.q_cov_acc_long * dt4; + const double q_cov_y = 0.25 * motion_params_.q_cov_acc_lat * dt4; const double q_cov_yaw = q_stddev_yaw_rate * q_stddev_yaw_rate * dt2; const double q_cov_vel = motion_params_.q_cov_acc_long * dt2; const double q_cov_slip = q_cov_slip_rate * dt2; diff --git a/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp index 807c24f2e962a..b19af1d26d162 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp @@ -64,11 +64,11 @@ void CTRVMotionModel::setMotionParams( const double & q_stddev_vel, const double & q_stddev_wz) { // set process noise covariance parameters - motion_params_.q_cov_x = std::pow(q_stddev_x, 2.0); - motion_params_.q_cov_y = std::pow(q_stddev_y, 2.0); - motion_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0); - motion_params_.q_cov_vel = std::pow(q_stddev_vel, 2.0); - motion_params_.q_cov_wz = std::pow(q_stddev_wz, 2.0); + motion_params_.q_cov_x = q_stddev_x * q_stddev_x; + motion_params_.q_cov_y = q_stddev_y * q_stddev_y; + motion_params_.q_cov_yaw = q_stddev_yaw * q_stddev_yaw; + motion_params_.q_cov_vel = q_stddev_vel * q_stddev_vel; + motion_params_.q_cov_wz = q_stddev_wz * q_stddev_wz; } void CTRVMotionModel::setMotionLimits(const double & max_vel, const double & max_wz) @@ -296,11 +296,12 @@ bool CTRVMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) cons A(IDX::YAW, IDX::WZ) = dt; // Process noise covariance Q - const double q_cov_x = motion_params_.q_cov_x * dt * dt; - const double q_cov_y = motion_params_.q_cov_y * dt * dt; - const double q_cov_yaw = motion_params_.q_cov_yaw * dt * dt; - const double q_cov_vel = motion_params_.q_cov_vel * dt * dt; - const double q_cov_wz = motion_params_.q_cov_wz * dt * dt; + const double dt2 = dt * dt; + const double q_cov_x = motion_params_.q_cov_x * dt2; + const double q_cov_y = motion_params_.q_cov_y * dt2; + const double q_cov_yaw = motion_params_.q_cov_yaw * dt2; + const double q_cov_vel = motion_params_.q_cov_vel * dt2; + const double q_cov_wz = motion_params_.q_cov_wz * dt2; Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(DIM, DIM); // Rotate the covariance matrix according to the vehicle yaw // because q_cov_x and y are in the vehicle coordinate system. diff --git a/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp index ebd59735cc8f1..231ba73796ed9 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp @@ -64,10 +64,10 @@ void CVMotionModel::setMotionParams( const double & q_stddev_vy) { // set process noise covariance parameters - motion_params_.q_cov_x = std::pow(q_stddev_x, 2.0); - motion_params_.q_cov_y = std::pow(q_stddev_y, 2.0); - motion_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); - motion_params_.q_cov_vy = std::pow(q_stddev_vy, 2.0); + motion_params_.q_cov_x = q_stddev_x * q_stddev_x; + motion_params_.q_cov_y = q_stddev_y * q_stddev_y; + motion_params_.q_cov_vx = q_stddev_vx * q_stddev_vx; + motion_params_.q_cov_vy = q_stddev_vy * q_stddev_vy; } void CVMotionModel::setMotionLimits(const double & max_vx, const double & max_vy)