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 8f63dee9f872e..47ed52f4155e0 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 @@ -74,7 +74,7 @@ struct MotionProcessNoise double yaw_rate_max{0.0}; // [rad/s] uncertain yaw rate, maximum double acc_long{0.0}; // [m/s^2] uncertain longitudinal acceleration double acc_lat{0.0}; // [m/s^2] uncertain lateral acceleration - double rotate_rate{0.0}; // [rad/s^2] uncertain rotation change rate + double acc_turn{0.0}; // [rad/s^2] uncertain rotation acceleration }; struct MotionProcessLimit { @@ -268,17 +268,17 @@ class ObjectModel process_noise.vel_lat = 0.5; process_noise.yaw_rate = deg2rad(20.0); process_noise.acc_long = const_g * 0.3; - process_noise.rotate_rate = deg2rad(30.0); + process_noise.acc_turn = deg2rad(30.0); process_limit.vel_long_max = kmph2mps(100.0); process_limit.yaw_rate_max = deg2rad(30.0); // initial covariance - initial_covariance.pos_x = sq(1.0); - initial_covariance.pos_y = sq(0.3); - initial_covariance.yaw = sq(deg2rad(25.0)); - initial_covariance.vel_long = sq(kmph2mps(1000.0)); - initial_covariance.vel_lat = sq(0.2); + initial_covariance.pos_x = sq(2.0); + initial_covariance.pos_y = sq(2.0); + initial_covariance.yaw = sq(deg2rad(1000.0)); + initial_covariance.vel_long = sq(kmph2mps(120.0)); + initial_covariance.yaw_rate = sq(deg2rad(360.0)); // measurement noise model measurement_covariance.pos_x = sq(0.4); 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 262a6cfce8803..a30ff67d9a837 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -176,10 +176,10 @@ autoware_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingObject( auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; const double cos_yaw = std::cos(pose_yaw); const double sin_yaw = std::sin(pose_yaw); - const double sin_2yaw = std::sin(2.0f * pose_yaw); + const double sin_2yaw = std::sin(2.0 * pose_yaw); pose_cov[XYZRPY_COV_IDX::X_X] = r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x - pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y + pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5 * (r_cov_x - r_cov_y) * sin_2yaw; // x - y pose_cov[XYZRPY_COV_IDX::Y_Y] = r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y pose_cov[XYZRPY_COV_IDX::Y_X] = pose_cov[XYZRPY_COV_IDX::X_Y]; // y - x 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 58037e858f701..658852d2a41bd 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 @@ -216,10 +216,10 @@ autoware_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObje auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; const double cos_yaw = std::cos(pose_yaw); const double sin_yaw = std::sin(pose_yaw); - const double sin_2yaw = std::sin(2.0f * pose_yaw); + const double sin_2yaw = std::sin(2.0 * pose_yaw); pose_cov[XYZRPY_COV_IDX::X_X] = r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x - pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y + pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5 * (r_cov_x - r_cov_y) * sin_2yaw; // x - y pose_cov[XYZRPY_COV_IDX::Y_Y] = r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y pose_cov[XYZRPY_COV_IDX::Y_X] = pose_cov[XYZRPY_COV_IDX::X_Y]; // y - x 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 080a3cefcc50b..fbd5637d9505d 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 @@ -217,10 +217,10 @@ autoware_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingO auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; const double cos_yaw = std::cos(pose_yaw); const double sin_yaw = std::sin(pose_yaw); - const double sin_2yaw = std::sin(2.0f * pose_yaw); + const double sin_2yaw = std::sin(2.0 * pose_yaw); pose_cov[XYZRPY_COV_IDX::X_X] = r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x - pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y + pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5 * (r_cov_x - r_cov_y) * sin_2yaw; // x - y pose_cov[XYZRPY_COV_IDX::Y_Y] = r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y pose_cov[XYZRPY_COV_IDX::Y_X] = pose_cov[XYZRPY_COV_IDX::X_Y]; // y - x diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index cfc4ae236f28a..e453fd140b552 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -86,15 +86,16 @@ PedestrianTracker::PedestrianTracker( const double q_stddev_y = object_model_.process_noise.vel_lat; const double q_stddev_yaw = object_model_.process_noise.yaw_rate; const double q_stddev_vx = object_model_.process_noise.acc_long; - const double q_stddev_wz = object_model_.process_noise.rotate_rate; + const double q_stddev_wz = object_model_.process_noise.acc_turn; motion_model_.setMotionParams(q_stddev_x, q_stddev_y, q_stddev_yaw, q_stddev_vx, q_stddev_wz); } // Set motion limits - motion_model_.setMotionLimits( - tier4_autoware_utils::kmph2mps(100), /* [m/s] maximum velocity */ - 30.0 /* [deg/s] maximum turn rate */ - ); + { + const double max_vel = object_model_.process_limit.vel_long_max; + const double max_turn_rate = object_model_.process_limit.yaw_rate_max; + motion_model_.setMotionLimits(max_vel, max_turn_rate); // maximum velocity and slip angle + } // Set initial state { @@ -102,26 +103,13 @@ PedestrianTracker::PedestrianTracker( const double x = object.kinematics.pose_with_covariance.pose.position.x; const double y = object.kinematics.pose_with_covariance.pose.position.y; const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - auto pose_cov = object.kinematics.pose_with_covariance.covariance; - double vel = 0.0; - double wz = 0.0; - double vel_cov = 10.0; - double wz_cov = 10.0; - - if (object.kinematics.has_twist) { - vel = object.kinematics.twist_with_covariance.twist.linear.x; - wz = object.kinematics.twist_with_covariance.twist.angular.z; - } + auto pose_cov = object.kinematics.pose_with_covariance.covariance; if (!object.kinematics.has_position_covariance) { // initial state covariance - constexpr double p0_stddev_x = 2.0; // in object coordinate [m] - constexpr double p0_stddev_y = 2.0; // in object coordinate [m] - constexpr double p0_stddev_yaw = - tier4_autoware_utils::deg2rad(1000); // in map coordinate [rad] - constexpr double p0_cov_x = p0_stddev_x * p0_stddev_x; - constexpr double p0_cov_y = p0_stddev_y * p0_stddev_y; - constexpr double p0_cov_yaw = p0_stddev_yaw * p0_stddev_yaw; + const auto & p0_cov_x = object_model_.initial_covariance.pos_x; + const auto & p0_cov_y = object_model_.initial_covariance.pos_y; + const auto & p0_cov_yaw = object_model_.initial_covariance.yaw; const double cos_yaw = std::cos(yaw); const double sin_yaw = std::sin(yaw); @@ -133,14 +121,16 @@ PedestrianTracker::PedestrianTracker( pose_cov[XYZRPY_COV_IDX::YAW_YAW] = p0_cov_yaw; } - if (!object.kinematics.has_twist_covariance) { - constexpr double p0_stddev_vel = - tier4_autoware_utils::kmph2mps(120); // in object coordinate [m/s] - constexpr double p0_stddev_wz = - tier4_autoware_utils::deg2rad(360); // in object coordinate [rad/s] - vel_cov = std::pow(p0_stddev_vel, 2.0); - wz_cov = std::pow(p0_stddev_wz, 2.0); - } else { + double vel = 0.0; + double wz = 0.0; + if (object.kinematics.has_twist) { + vel = object.kinematics.twist_with_covariance.twist.linear.x; + wz = object.kinematics.twist_with_covariance.twist.angular.z; + } + + double vel_cov = object_model_.initial_covariance.vel_long; + double wz_cov = object_model_.initial_covariance.yaw_rate; + if (object.kinematics.has_twist_covariance) { vel_cov = object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::X_X]; wz_cov = object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::YAW_YAW]; } @@ -178,10 +168,10 @@ autoware_perception_msgs::msg::DetectedObject PedestrianTracker::getUpdatingObje auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; const double cos_yaw = std::cos(pose_yaw); const double sin_yaw = std::sin(pose_yaw); - const double sin_2yaw = std::sin(2.0f * pose_yaw); + const double sin_2yaw = std::sin(2.0 * pose_yaw); pose_cov[XYZRPY_COV_IDX::X_X] = r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x - pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y + pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5 * (r_cov_x - r_cov_y) * sin_2yaw; // x - y pose_cov[XYZRPY_COV_IDX::Y_Y] = r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y pose_cov[XYZRPY_COV_IDX::Y_X] = pose_cov[XYZRPY_COV_IDX::X_Y]; // y - x 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 c9ecfff193767..38d2135e354dc 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 @@ -296,11 +296,11 @@ bool CTRVMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) cons A(IDX::YAW, IDX::WZ) = dt; // Process noise covariance Q - const double q_cov_x = std::pow(0.5 * motion_params_.q_cov_x * dt, 2); - const double q_cov_y = std::pow(0.5 * motion_params_.q_cov_y * dt, 2); - const double q_cov_yaw = std::pow(motion_params_.q_cov_yaw * dt, 2); - const double q_cov_vel = std::pow(motion_params_.q_cov_vel * dt, 2); - const double q_cov_wz = std::pow(motion_params_.q_cov_wz * dt, 2); + 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; 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.