diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp index f6ce2842388c6..472b8f2a309f3 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp @@ -47,8 +47,6 @@ class BicycleMotionModel : public MotionModel { double q_stddev_acc_long; double q_stddev_acc_lat; - double q_cov_acc_long; - double q_cov_acc_lat; double q_stddev_yaw_rate_min; double q_stddev_yaw_rate_max; double q_cov_slip_rate_min; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model.hpp index 6a771344bb36b..4175fa3a16429 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model.hpp @@ -99,8 +99,8 @@ struct BicycleModelState { double init_slip_angle_cov{0.0}; // [rad^2/s^2] initial slip angle covariance double slip_angle_max{0.0}; // [rad] max slip angle - double slip_rate_stddev_min{0.0}; // [rad/s] uncertain slip angle change rate, minimum - double slip_rate_stddev_max{0.0}; // [rad/s] uncertain slip angle change rate, maximum + double slip_rate_cov_min{0.0}; // [rad/s] uncertain slip angle change rate, minimum + double slip_rate_cov_max{0.0}; // [rad/s] uncertain slip angle change rate, maximum double wheel_pos_ratio_front{0.0}; // [-] double wheel_pos_ratio_rear{0.0}; // [-] double wheel_pos_front_min{0.0}; // [m] @@ -134,8 +134,8 @@ class ObjectModel process_noise.acc_long = const_g * 0.35; process_noise.acc_lat = const_g * 0.15; - process_noise.yaw_rate_min = deg2rad(1.5); - process_noise.yaw_rate_max = deg2rad(15.0); + process_noise.yaw_rate_min = 1.5; // deg2rad(1.5); + process_noise.yaw_rate_max = 15.0; // deg2rad(15.0); process_limit.acc_long_max = const_g; process_limit.acc_lat_max = const_g; @@ -156,9 +156,9 @@ class ObjectModel // bicycle motion model bicycle_state.init_slip_angle_cov = sq(deg2rad(5.0)); - bicycle_state.slip_angle_max = deg2rad(30.0); - bicycle_state.slip_rate_stddev_min = deg2rad(0.3); - bicycle_state.slip_rate_stddev_max = deg2rad(10.0); + bicycle_state.slip_angle_max = 30.0; // deg2rad(30.0); + bicycle_state.slip_rate_cov_min = 0.3; // sq(deg2rad(0.3)); + bicycle_state.slip_rate_cov_max = 10.0; // sq(deg2rad(10.0)); bicycle_state.wheel_pos_ratio_front = 0.3; bicycle_state.wheel_pos_ratio_rear = 0.25; bicycle_state.wheel_pos_front_min = 1.0; @@ -178,8 +178,8 @@ class ObjectModel process_noise.acc_long = const_g * 0.35; process_noise.acc_lat = const_g * 0.15; - process_noise.yaw_rate_min = deg2rad(1.5); - process_noise.yaw_rate_max = deg2rad(15.0); + process_noise.yaw_rate_min = 1.5; // deg2rad(1.5); + process_noise.yaw_rate_max = 15.0; // deg2rad(15.0); process_limit.acc_long_max = const_g; process_limit.acc_lat_max = const_g; @@ -200,9 +200,9 @@ class ObjectModel // bicycle motion model bicycle_state.init_slip_angle_cov = sq(deg2rad(5.0)); - bicycle_state.slip_angle_max = deg2rad(30.0); - bicycle_state.slip_rate_stddev_min = deg2rad(0.3); - bicycle_state.slip_rate_stddev_max = deg2rad(10.0); + bicycle_state.slip_angle_max = 30.0; // deg2rad(30.0); + bicycle_state.slip_rate_cov_min = 0.3; // sq(deg2rad(0.3)); + bicycle_state.slip_rate_cov_max = 10.0; // sq(deg2rad(10.0)); bicycle_state.wheel_pos_ratio_front = 0.3; bicycle_state.wheel_pos_ratio_rear = 0.25; bicycle_state.wheel_pos_front_min = 1.5; @@ -222,8 +222,8 @@ class ObjectModel process_noise.acc_long = const_g * 0.35; process_noise.acc_lat = const_g * 0.15; - process_noise.yaw_rate_min = deg2rad(5.0); - process_noise.yaw_rate_max = deg2rad(15.0); + process_noise.yaw_rate_min = 5.0; // deg2rad(5.0); + process_noise.yaw_rate_max = 15.0; // deg2rad(15.0); process_limit.acc_long_max = const_g; process_limit.acc_lat_max = const_g; @@ -244,9 +244,9 @@ class ObjectModel // bicycle motion model bicycle_state.init_slip_angle_cov = sq(deg2rad(5.0)); - bicycle_state.slip_angle_max = deg2rad(30.0); - bicycle_state.slip_rate_stddev_min = deg2rad(1.0); - bicycle_state.slip_rate_stddev_max = deg2rad(10.0); + bicycle_state.slip_angle_max = 30.0; // deg2rad(30.0); + bicycle_state.slip_rate_cov_min = 1.0; // sq(deg2rad(1.0)); + bicycle_state.slip_rate_cov_max = 10.0; // sq(deg2rad(10.0)); bicycle_state.wheel_pos_ratio_front = 0.3; bicycle_state.wheel_pos_ratio_rear = 0.3; bicycle_state.wheel_pos_front_min = 0.3; 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 8419259f58ebe..83e6bfb770b53 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -78,8 +78,8 @@ BicycleTracker::BicycleTracker( const double q_stddev_acc_lat = object_model_.process_noise.acc_lat; const double q_stddev_yaw_rate_min = object_model_.process_noise.yaw_rate_min; const double q_stddev_yaw_rate_max = object_model_.process_noise.yaw_rate_max; - const double q_stddev_slip_rate_min = object_model_.bicycle_state.slip_rate_stddev_min; - const double q_stddev_slip_rate_max = object_model_.bicycle_state.slip_rate_stddev_max; + const double q_stddev_slip_rate_min = object_model_.bicycle_state.slip_rate_cov_min; + const double q_stddev_slip_rate_max = object_model_.bicycle_state.slip_rate_cov_max; const double q_max_slip_angle = object_model_.bicycle_state.slip_angle_max; const double lf_ratio = object_model_.bicycle_state.wheel_pos_ratio_front; const double lf_min = object_model_.bicycle_state.wheel_pos_front_min; 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 d8a0346c9538d..fabbacb8bc037 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 @@ -94,8 +94,8 @@ BigVehicleTracker::BigVehicleTracker( const double q_stddev_acc_lat = object_model_.process_noise.acc_lat; const double q_stddev_yaw_rate_min = object_model_.process_noise.yaw_rate_min; const double q_stddev_yaw_rate_max = object_model_.process_noise.yaw_rate_max; - const double q_stddev_slip_rate_min = object_model_.bicycle_state.slip_rate_stddev_min; - const double q_stddev_slip_rate_max = object_model_.bicycle_state.slip_rate_stddev_max; + const double q_stddev_slip_rate_min = object_model_.bicycle_state.slip_rate_cov_min; + const double q_stddev_slip_rate_max = object_model_.bicycle_state.slip_rate_cov_max; const double q_max_slip_angle = object_model_.bicycle_state.slip_angle_max; const double lf_ratio = object_model_.bicycle_state.wheel_pos_ratio_front; const double lf_min = object_model_.bicycle_state.wheel_pos_front_min; 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 aaff159662349..f56c869be6fbe 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 @@ -95,8 +95,8 @@ NormalVehicleTracker::NormalVehicleTracker( const double q_stddev_acc_lat = object_model_.process_noise.acc_lat; const double q_stddev_yaw_rate_min = object_model_.process_noise.yaw_rate_min; const double q_stddev_yaw_rate_max = object_model_.process_noise.yaw_rate_max; - const double q_stddev_slip_rate_min = object_model_.bicycle_state.slip_rate_stddev_min; - const double q_stddev_slip_rate_max = object_model_.bicycle_state.slip_rate_stddev_max; + const double q_stddev_slip_rate_min = object_model_.bicycle_state.slip_rate_cov_min; + const double q_stddev_slip_rate_max = object_model_.bicycle_state.slip_rate_cov_max; const double q_max_slip_angle = object_model_.bicycle_state.slip_angle_max; const double lf_ratio = object_model_.bicycle_state.wheel_pos_ratio_front; const double lf_min = object_model_.bicycle_state.wheel_pos_front_min; 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 10e77088ff342..0f3287c448c0a 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 @@ -44,16 +44,11 @@ void BicycleMotionModel::setDefaultParams() // set default motion parameters constexpr double q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration constexpr double q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration - constexpr double q_stddev_yaw_rate_min = - autoware_universe_utils::deg2rad(1.5); // [rad/s] uncertain yaw change rate - constexpr double q_stddev_yaw_rate_max = - autoware_universe_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate - constexpr double q_stddev_slip_rate_min = - autoware_universe_utils::deg2rad(0.3); // [rad/s] uncertain slip angle change rate - constexpr double q_stddev_slip_rate_max = - autoware_universe_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate - constexpr double q_max_slip_angle = - autoware_universe_utils::deg2rad(30.0); // [rad] max slip angle + constexpr double q_stddev_yaw_rate_min = 1.5; // [deg/s] uncertain yaw change rate + constexpr double q_stddev_yaw_rate_max = 15.0; // [deg/s] uncertain yaw change rate + constexpr double q_stddev_slip_rate_min = 0.3; // [deg/s] uncertain slip angle change rate + constexpr double q_stddev_slip_rate_max = 10.0; // [deg/s] uncertain slip angle change rate + constexpr double q_max_slip_angle = 30.0; // [deg] max slip angle // extended state parameters constexpr double lf_ratio = 0.3; // 30% front from the center constexpr double lf_min = 1.0; // minimum of 1.0m @@ -84,13 +79,13 @@ void BicycleMotionModel::setMotionParams( // set process noise covariance parameters motion_params_.q_stddev_acc_long = q_stddev_acc_long; motion_params_.q_stddev_acc_lat = q_stddev_acc_lat; - motion_params_.q_cov_acc_long = q_stddev_acc_long * q_stddev_acc_long; - motion_params_.q_cov_acc_lat = q_stddev_acc_lat * q_stddev_acc_lat; - motion_params_.q_stddev_yaw_rate_min = q_stddev_yaw_rate_min; - motion_params_.q_stddev_yaw_rate_max = q_stddev_yaw_rate_max; - motion_params_.q_cov_slip_rate_min = q_stddev_slip_rate_min * q_stddev_slip_rate_min; - motion_params_.q_cov_slip_rate_max = q_stddev_slip_rate_max * q_stddev_slip_rate_max; - motion_params_.q_max_slip_angle = q_max_slip_angle; + motion_params_.q_stddev_yaw_rate_min = autoware_universe_utils::deg2rad(q_stddev_yaw_rate_min); + motion_params_.q_stddev_yaw_rate_max = autoware_universe_utils::deg2rad(q_stddev_yaw_rate_max); + motion_params_.q_cov_slip_rate_min = + std::pow(autoware_universe_utils::deg2rad(q_stddev_slip_rate_min), 2.0); + motion_params_.q_cov_slip_rate_max = + std::pow(autoware_universe_utils::deg2rad(q_stddev_slip_rate_max), 2.0); + motion_params_.q_max_slip_angle = autoware_universe_utils::deg2rad(q_max_slip_angle); constexpr double minimum_wheel_pos = 0.01; // minimum of 0.01m if (lf_min < minimum_wheel_pos || lr_min < minimum_wheel_pos) { @@ -359,8 +354,10 @@ bool BicycleMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) c A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt; // Process noise covariance Q - double q_stddev_yaw_rate = motion_params_.q_stddev_yaw_rate_min; - if (vel > 0.01) { + double q_stddev_yaw_rate{0.0}; + if (vel <= 0.01) { + q_stddev_yaw_rate = motion_params_.q_stddev_yaw_rate_min; + } else { /* uncertainty of the yaw rate is limited by the following: * - centripetal acceleration a_lat : d(yaw)/dt = w = a_lat/v * - or maximum slip angle slip_max : w = v*sin(slip_max)/l_r @@ -368,9 +365,8 @@ bool BicycleMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) c q_stddev_yaw_rate = std::min( motion_params_.q_stddev_acc_lat / vel, vel * std::sin(motion_params_.q_max_slip_angle) / lr_); // [rad/s] - q_stddev_yaw_rate = std::clamp( - q_stddev_yaw_rate, motion_params_.q_stddev_yaw_rate_min, - motion_params_.q_stddev_yaw_rate_max); + q_stddev_yaw_rate = std::min(q_stddev_yaw_rate, motion_params_.q_stddev_yaw_rate_max); + q_stddev_yaw_rate = std::max(q_stddev_yaw_rate, motion_params_.q_stddev_yaw_rate_min); } double q_cov_slip_rate{0.0}; if (vel <= 0.01) { @@ -388,13 +384,11 @@ bool BicycleMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) c q_cov_slip_rate = std::min(q_cov_slip_rate, motion_params_.q_cov_slip_rate_max); q_cov_slip_rate = std::max(q_cov_slip_rate, motion_params_.q_cov_slip_rate_min); } - 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_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; + const double q_cov_x = std::pow(0.5 * motion_params_.q_stddev_acc_long * dt * dt, 2); + const double q_cov_y = std::pow(0.5 * motion_params_.q_stddev_acc_lat * dt * dt, 2); + const double q_cov_yaw = std::pow(q_stddev_yaw_rate * dt, 2); + const double q_cov_vel = std::pow(motion_params_.q_stddev_acc_long * dt, 2); + const double q_cov_slip = q_cov_slip_rate * dt * dt; Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(DIM, DIM); // Rotate the covariance matrix according to the vehicle yaw