diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp index 8a30af39870b5..ad268283d5890 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -32,23 +32,17 @@ class BicycleTracker : public Tracker private: KalmanFilter ekf_; rclcpp::Time last_update_time_; - enum IDX { - X = 0, - Y = 1, - YAW = 2, - VX = 3, - WZ = 4, - }; + enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, SLIP = 4 }; struct EkfParams { char dim_x = 5; float q_cov_x; float q_cov_y; float q_cov_yaw; - float q_cov_wz; + float q_cov_slip; float q_cov_vx; float p0_cov_vx; - float p0_cov_wz; + float p0_cov_slip; float r_cov_x; float r_cov_y; float r_cov_yaw; @@ -58,7 +52,9 @@ class BicycleTracker : public Tracker } ekf_params_; double max_vx_; - double max_wz_; + double max_slip_; + double lf_; + double lr_; float z_; private: diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp index 614faa6a7d5b4..8a9b6adfc9cd5 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp @@ -32,23 +32,17 @@ class BigVehicleTracker : public Tracker private: KalmanFilter ekf_; rclcpp::Time last_update_time_; - enum IDX { - X = 0, - Y = 1, - YAW = 2, - VX = 3, - WZ = 4, - }; + enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, SLIP = 4 }; struct EkfParams { char dim_x = 5; float q_cov_x; float q_cov_y; float q_cov_yaw; - float q_cov_wz; + float q_cov_slip; float q_cov_vx; float p0_cov_vx; - float p0_cov_wz; + float p0_cov_slip; float r_cov_x; float r_cov_y; float r_cov_yaw; @@ -58,7 +52,9 @@ class BigVehicleTracker : public Tracker float p0_cov_yaw; } ekf_params_; double max_vx_; - double max_wz_; + double max_slip_; + double lf_; + double lr_; float z_; double velocity_deviation_threshold_; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp index 8175f20c5d671..79e9a20a10421 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp @@ -33,13 +33,7 @@ class NormalVehicleTracker : public Tracker private: KalmanFilter ekf_; rclcpp::Time last_update_time_; - enum IDX { - X = 0, - Y = 1, - YAW = 2, - VX = 3, - WZ = 4, - }; + enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, SLIP = 4 }; struct EkfParams { @@ -47,10 +41,10 @@ class NormalVehicleTracker : public Tracker float q_cov_x; float q_cov_y; float q_cov_yaw; - float q_cov_wz; + float q_cov_slip; float q_cov_vx; float p0_cov_vx; - float p0_cov_wz; + float p0_cov_slip; float r_cov_x; float r_cov_y; float r_cov_yaw; @@ -61,7 +55,9 @@ class NormalVehicleTracker : public Tracker } ekf_params_; double max_vx_; - double max_wz_; + double max_slip_; + double lf_; + double lr_; float z_; double velocity_deviation_threshold_; 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 20dbd51e88eaf..5de81eb2d4538 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -55,7 +55,7 @@ BicycleTracker::BicycleTracker( float q_stddev_y = 0.6; // [m/s] float q_stddev_yaw = tier4_autoware_utils::deg2rad(10); // [rad/s] float q_stddev_vx = tier4_autoware_utils::kmph2mps(10); // [m/(s*s)] - float q_stddev_wz = tier4_autoware_utils::deg2rad(25); // [rad/(s*s)] + float q_stddev_slip = tier4_autoware_utils::deg2rad(25); // [rad/(s*s)] float r_stddev_x = 0.6; // [m] float r_stddev_y = 0.4; // [m] float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] @@ -63,12 +63,12 @@ BicycleTracker::BicycleTracker( float p0_stddev_y = 0.5; // [m/s] float p0_stddev_yaw = tier4_autoware_utils::deg2rad(1000); // [rad/s] float p0_stddev_vx = tier4_autoware_utils::kmph2mps(1000); // [m/(s*s)] - float p0_stddev_wz = tier4_autoware_utils::deg2rad(10); // [rad/(s*s)] + float p0_stddev_slip = tier4_autoware_utils::deg2rad(10); // [rad/(s*s)] ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0); ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0); ekf_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0); ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); - ekf_params_.q_cov_wz = std::pow(q_stddev_wz, 2.0); + ekf_params_.q_cov_slip = std::pow(q_stddev_slip, 2.0); ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); @@ -76,9 +76,9 @@ BicycleTracker::BicycleTracker( ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0); ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0); - ekf_params_.p0_cov_wz = std::pow(p0_stddev_wz, 2.0); + ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); max_vx_ = tier4_autoware_utils::kmph2mps(100); // [m/s] - max_wz_ = tier4_autoware_utils::deg2rad(30); // [rad/s] + max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad/s] // initialize X matrix Eigen::MatrixXd X(ekf_params_.dim_x, 1); @@ -87,11 +87,10 @@ BicycleTracker::BicycleTracker( X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); if (object.kinematics.has_twist) { X(IDX::VX) = object.kinematics.twist_with_covariance.twist.linear.x; - X(IDX::WZ) = object.kinematics.twist_with_covariance.twist.angular.z; } else { X(IDX::VX) = 0.0; - X(IDX::WZ) = 0.0; } + X(IDX::SLIP) = 0.0; // initialize P matrix Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); @@ -110,7 +109,7 @@ BicycleTracker::BicycleTracker( P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y); P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw; P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; - P(IDX::WZ, IDX::WZ) = ekf_params_.p0_cov_wz; + P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } else { P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; P(IDX::X, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; @@ -121,12 +120,10 @@ BicycleTracker::BicycleTracker( if (object.kinematics.has_twist_covariance) { P(IDX::VX, IDX::VX) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - P(IDX::WZ, IDX::WZ) = - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; } else { P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; - P(IDX::WZ, IDX::WZ) = ekf_params_.p0_cov_wz; } + P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { @@ -136,6 +133,11 @@ BicycleTracker::BicycleTracker( bounding_box_ = {1.0, 0.5, 1.7}; } ekf_.init(X, P); + + // Set lf, lr + double point_ratio = 0.5; // comes to front if larger + lf_ = bounding_box_.length * point_ratio; + lr_ = bounding_box_.length * (1.0 - point_ratio); } bool BicycleTracker::predict(const rclcpp::Time & time) @@ -150,47 +152,53 @@ bool BicycleTracker::predict(const rclcpp::Time & time) bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const { - /* == Nonlinear model == + /* == Nonlinear model == static bicycle model * - * x_{k+1} = x_k + vx_k * cos(yaw_k) * dt - * y_{k+1} = y_k + vx_k * sin(yaw_k) * dt - * yaw_{k+1} = yaw_k + (wz_k) * dt + * x_{k+1} = x_k + vx_k * cos(yaw_k + slip_k) * dt + * y_{k+1} = y_k + vx_k * sin(yaw_k + slip_k) * dt + * yaw_{k+1} = yaw_k + vx_k / l_r * sin(slip_k) * dt * vx_{k+1} = vx_k - * wz_{k+1} = wz_k + * slip_{k+1} = slip_k * */ /* == Linearized model == * - * A = [ 1, 0, -vx*sin(yaw)*dt, cos(yaw)*dt, 0] - * [ 0, 1, vx*cos(yaw)*dt, sin(yaw)*dt, 0] - * [ 0, 0, 1, 0, dt] - * [ 0, 0, 0, 1, 0] - * [ 0, 0, 0, 0, 1] + * A = [ 1, 0, -vx*sin(yaw+slip)*dt, cos(yaw+slip)*dt, -vx*sin(yaw+slip)*dt] + * [ 0, 1, vx*cos(yaw+slip)*dt, sin(yaw+slip)*dt, vx*cos(yaw+slip)*dt] + * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vx/l_r*cos(slip)*dt] + * [ 0, 0, 0, 1, 0] + * [ 0, 0, 0, 0, 1] */ // X t Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state ekf.getX(X_t); - const double cos_yaw = std::cos(X_t(IDX::YAW)); - const double sin_yaw = std::sin(X_t(IDX::YAW)); + const double cos_yaw = std::cos(X_t(IDX::YAW) + X_t(IDX::SLIP)); + const double sin_yaw = std::sin(X_t(IDX::YAW) + X_t(IDX::SLIP)); + const double cos_slip = std::cos(X_t(IDX::SLIP)); + const double sin_slip = std::sin(X_t(IDX::SLIP)); + const double vx = X_t(IDX::VX); const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); // X t+1 - Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state - X_next_t(IDX::X) = X_t(IDX::X) + X_t(IDX::VX) * cos_yaw * dt; // dx = v * cos(yaw) - X_next_t(IDX::Y) = X_t(IDX::Y) + X_t(IDX::VX) * sin_yaw * dt; // dy = v * sin(yaw) - X_next_t(IDX::YAW) = X_t(IDX::YAW) + (X_t(IDX::WZ)) * dt; // dyaw = omega + Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state + X_next_t(IDX::X) = X_t(IDX::X) + vx * cos_yaw * dt; // dx = v * cos(yaw) + X_next_t(IDX::Y) = X_t(IDX::Y) + vx * sin_yaw * dt; // dy = v * sin(yaw) + X_next_t(IDX::YAW) = X_t(IDX::YAW) + vx / lr_ * sin_slip * dt; // dyaw = omega X_next_t(IDX::VX) = X_t(IDX::VX); - X_next_t(IDX::WZ) = X_t(IDX::WZ); + X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // A Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); - A(IDX::X, IDX::YAW) = -X_t(IDX::VX) * sin_yaw * dt; + A(IDX::X, IDX::YAW) = -vx * sin_yaw * dt; A(IDX::X, IDX::VX) = cos_yaw * dt; - A(IDX::Y, IDX::YAW) = X_t(IDX::VX) * cos_yaw * dt; + A(IDX::X, IDX::SLIP) = -vx * sin_yaw * dt; + A(IDX::Y, IDX::YAW) = vx * cos_yaw * dt; A(IDX::Y, IDX::VX) = sin_yaw * dt; - A(IDX::YAW, IDX::WZ) = dt; + A(IDX::Y, IDX::SLIP) = vx * cos_yaw * dt; + A(IDX::YAW, IDX::VX) = 1.0 / lr_ * sin_slip * dt; + A(IDX::YAW, IDX::SLIP) = vx / lr_ * cos_slip * dt; // Q Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); @@ -204,7 +212,7 @@ bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); Q(IDX::YAW, IDX::YAW) = ekf_params_.q_cov_yaw * dt * dt; Q(IDX::VX, IDX::VX) = ekf_params_.q_cov_vx * dt * dt; - Q(IDX::WZ, IDX::WZ) = ekf_params_.q_cov_wz * dt * dt; + Q(IDX::SLIP, IDX::SLIP) = ekf_params_.q_cov_slip * dt * dt; Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); @@ -315,8 +323,8 @@ bool BicycleTracker::measureWithPose( if (!(-max_vx_ <= X_t(IDX::VX) && X_t(IDX::VX) <= max_vx_)) { X_t(IDX::VX) = X_t(IDX::VX) < 0 ? -max_vx_ : max_vx_; } - if (!(-max_wz_ <= X_t(IDX::WZ) && X_t(IDX::WZ) <= max_wz_)) { - X_t(IDX::WZ) = X_t(IDX::WZ) < 0 ? -max_wz_ : max_wz_; + if (!(-max_slip_ <= X_t(IDX::SLIP) && X_t(IDX::SLIP) <= max_slip_)) { + X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -max_slip_ : max_slip_; } ekf_.init(X_t, P_t); } @@ -420,7 +428,8 @@ bool BicycleTracker::getTrackedObject( // twist twist_with_cov.twist.linear.x = X_t(IDX::VX); - twist_with_cov.twist.angular.z = X_t(IDX::WZ); + twist_with_cov.twist.angular.z = + X_t(IDX::VX) / lr_ * std::sin(X_t(IDX::SLIP)); // yaw_rate = vx_k / l_r * sin(slip_k) // twist covariance constexpr double vy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative @@ -429,11 +438,11 @@ bool BicycleTracker::getTrackedObject( twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VX, IDX::VX); twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = vy_cov; twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = P(IDX::VX, IDX::WZ); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = P(IDX::WZ, IDX::VX); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = P(IDX::VX, IDX::SLIP); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = P(IDX::SLIP, IDX::VX); twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::WZ, IDX::WZ); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::SLIP, IDX::SLIP); // set shape object.shape.dimensions.x = bounding_box_.length; 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 06200df9400f6..12086daa23463 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 @@ -53,7 +53,7 @@ BigVehicleTracker::BigVehicleTracker( float q_stddev_y = 1.5; // [m/s] float q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s] float q_stddev_vx = tier4_autoware_utils::kmph2mps(10); // [m/(s*s)] - float q_stddev_wz = tier4_autoware_utils::deg2rad(20); // [rad/(s*s)] + float q_stddev_slip = tier4_autoware_utils::deg2rad(20); // [rad/(s*s)] float r_stddev_x = 1.5; // [m] float r_stddev_y = 0.5; // [m] float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] @@ -62,12 +62,12 @@ BigVehicleTracker::BigVehicleTracker( float p0_stddev_y = 0.5; // [m] float p0_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] float p0_stddev_vx = tier4_autoware_utils::kmph2mps(1000); // [m/s] - float p0_stddev_wz = tier4_autoware_utils::deg2rad(10); // [rad/s] + float p0_stddev_slip = tier4_autoware_utils::deg2rad(10); // [rad/s] ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0); ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0); ekf_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0); ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); - ekf_params_.q_cov_wz = std::pow(q_stddev_wz, 2.0); + ekf_params_.q_cov_slip = std::pow(q_stddev_slip, 2.0); ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); @@ -76,9 +76,9 @@ BigVehicleTracker::BigVehicleTracker( ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0); ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0); - ekf_params_.p0_cov_wz = std::pow(p0_stddev_wz, 2.0); + ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); max_vx_ = tier4_autoware_utils::kmph2mps(100); // [m/s] - max_wz_ = tier4_autoware_utils::deg2rad(30); // [rad/s] + max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad] velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] // initialize X matrix @@ -86,12 +86,12 @@ BigVehicleTracker::BigVehicleTracker( X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y; X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + X(IDX::SLIP) = 0.0; if (object.kinematics.has_twist) { X(IDX::VX) = object.kinematics.twist_with_covariance.twist.linear.x; - X(IDX::WZ) = object.kinematics.twist_with_covariance.twist.angular.z; + // X(IDX::YAW) = object.kinematics.twist_with_covariance.twist.angular.z; } else { X(IDX::VX) = 0.0; - X(IDX::WZ) = 0.0; } // initialize P matrix @@ -110,7 +110,7 @@ BigVehicleTracker::BigVehicleTracker( P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y); P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw; P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; - P(IDX::WZ, IDX::WZ) = ekf_params_.p0_cov_wz; + P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } else { P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; P(IDX::X, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; @@ -121,12 +121,10 @@ BigVehicleTracker::BigVehicleTracker( if (object.kinematics.has_twist_covariance) { P(IDX::VX, IDX::VX) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - P(IDX::WZ, IDX::WZ) = - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; } else { P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; - P(IDX::WZ, IDX::WZ) = ekf_params_.p0_cov_wz; } + P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { @@ -149,6 +147,11 @@ BigVehicleTracker::BigVehicleTracker( /* calc nearest corner index*/ setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step + + // Set lf, lr + double point_ratio = 0.5; // comes to front if larger + lf_ = bounding_box_.length * point_ratio; + lr_ = bounding_box_.length * (1.0 - point_ratio); } bool BigVehicleTracker::predict(const rclcpp::Time & time) @@ -163,47 +166,53 @@ bool BigVehicleTracker::predict(const rclcpp::Time & time) bool BigVehicleTracker::predict(const double dt, KalmanFilter & ekf) const { - /* == Nonlinear model == + /* == Nonlinear model == static bicycle model * - * x_{k+1} = x_k + vx_k * cos(yaw_k) * dt - * y_{k+1} = y_k + vx_k * sin(yaw_k) * dt - * yaw_{k+1} = yaw_k + (wz_k) * dt + * x_{k+1} = x_k + vx_k * cos(yaw_k + slip_k) * dt + * y_{k+1} = y_k + vx_k * sin(yaw_k + slip_k) * dt + * yaw_{k+1} = yaw_k + vx_k / l_r * sin(slip_k) * dt * vx_{k+1} = vx_k - * wz_{k+1} = wz_k + * slip_{k+1} = slip_k * */ /* == Linearized model == * - * A = [ 1, 0, -vx*sin(yaw)*dt, cos(yaw)*dt, 0] - * [ 0, 1, vx*cos(yaw)*dt, sin(yaw)*dt, 0] - * [ 0, 0, 1, 0, dt] - * [ 0, 0, 0, 1, 0] - * [ 0, 0, 0, 0, 1] + * A = [ 1, 0, -vx*sin(yaw+slip)*dt, cos(yaw+slip)*dt, -vx*sin(yaw+slip)*dt] + * [ 0, 1, vx*cos(yaw+slip)*dt, sin(yaw+slip)*dt, vx*cos(yaw+slip)*dt] + * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vx/l_r*cos(slip)*dt] + * [ 0, 0, 0, 1, 0] + * [ 0, 0, 0, 0, 1] */ // X t Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state ekf.getX(X_t); - const double cos_yaw = std::cos(X_t(IDX::YAW)); - const double sin_yaw = std::sin(X_t(IDX::YAW)); + const double cos_yaw = std::cos(X_t(IDX::YAW) + X_t(IDX::SLIP)); + const double sin_yaw = std::sin(X_t(IDX::YAW) + X_t(IDX::SLIP)); + const double cos_slip = std::cos(X_t(IDX::SLIP)); + const double sin_slip = std::sin(X_t(IDX::SLIP)); + const double vx = X_t(IDX::VX); const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); // X t+1 - Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state - X_next_t(IDX::X) = X_t(IDX::X) + X_t(IDX::VX) * cos_yaw * dt; // dx = v * cos(yaw) - X_next_t(IDX::Y) = X_t(IDX::Y) + X_t(IDX::VX) * sin_yaw * dt; // dy = v * sin(yaw) - X_next_t(IDX::YAW) = X_t(IDX::YAW) + (X_t(IDX::WZ)) * dt; // dyaw = omega + Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state + X_next_t(IDX::X) = X_t(IDX::X) + vx * cos_yaw * dt; // dx = v * cos(yaw) + X_next_t(IDX::Y) = X_t(IDX::Y) + vx * sin_yaw * dt; // dy = v * sin(yaw) + X_next_t(IDX::YAW) = X_t(IDX::YAW) + vx / lr_ * sin_slip * dt; // dyaw = omega X_next_t(IDX::VX) = X_t(IDX::VX); - X_next_t(IDX::WZ) = X_t(IDX::WZ); + X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // A Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); - A(IDX::X, IDX::YAW) = -X_t(IDX::VX) * sin_yaw * dt; + A(IDX::X, IDX::YAW) = -vx * sin_yaw * dt; A(IDX::X, IDX::VX) = cos_yaw * dt; - A(IDX::Y, IDX::YAW) = X_t(IDX::VX) * cos_yaw * dt; + A(IDX::X, IDX::SLIP) = -vx * sin_yaw * dt; + A(IDX::Y, IDX::YAW) = vx * cos_yaw * dt; A(IDX::Y, IDX::VX) = sin_yaw * dt; - A(IDX::YAW, IDX::WZ) = dt; + A(IDX::Y, IDX::SLIP) = vx * cos_yaw * dt; + A(IDX::YAW, IDX::VX) = 1.0 / lr_ * sin_slip * dt; + A(IDX::YAW, IDX::SLIP) = vx / lr_ * cos_slip * dt; // Q Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); @@ -217,7 +226,7 @@ bool BigVehicleTracker::predict(const double dt, KalmanFilter & ekf) const Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); Q(IDX::YAW, IDX::YAW) = ekf_params_.q_cov_yaw * dt * dt; Q(IDX::VX, IDX::VX) = ekf_params_.q_cov_vx * dt * dt; - Q(IDX::WZ, IDX::WZ) = ekf_params_.q_cov_wz * dt * dt; + Q(IDX::SLIP, IDX::SLIP) = ekf_params_.q_cov_slip * dt * dt; Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); @@ -334,7 +343,7 @@ bool BigVehicleTracker::measureWithPose( RCLCPP_WARN(logger_, "Cannot update"); } - // normalize yaw and limit vx, wz + // normalize yaw and limit vx, slip { Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); @@ -344,8 +353,8 @@ bool BigVehicleTracker::measureWithPose( if (!(-max_vx_ <= X_t(IDX::VX) && X_t(IDX::VX) <= max_vx_)) { X_t(IDX::VX) = X_t(IDX::VX) < 0 ? -max_vx_ : max_vx_; } - if (!(-max_wz_ <= X_t(IDX::WZ) && X_t(IDX::WZ) <= max_wz_)) { - X_t(IDX::WZ) = X_t(IDX::WZ) < 0 ? -max_wz_ : max_wz_; + if (!(-max_slip_ <= X_t(IDX::SLIP) && X_t(IDX::SLIP) <= max_slip_)) { + X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -max_slip_ : max_slip_; } ekf_.init(X_t, P_t); } @@ -469,7 +478,8 @@ bool BigVehicleTracker::getTrackedObject( // twist twist_with_cov.twist.linear.x = X_t(IDX::VX); - twist_with_cov.twist.angular.z = X_t(IDX::WZ); + twist_with_cov.twist.angular.z = + X_t(IDX::VX) / lr_ * std::sin(X_t(IDX::SLIP)); // yaw_rate = vx_k / l_r * sin(slip_k) // twist covariance constexpr double vy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative @@ -478,11 +488,11 @@ bool BigVehicleTracker::getTrackedObject( twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VX, IDX::VX); twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = vy_cov; twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = P(IDX::VX, IDX::WZ); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = P(IDX::WZ, IDX::VX); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = P(IDX::VX, IDX::SLIP); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = P(IDX::SLIP, IDX::VX); twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::WZ, IDX::WZ); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::SLIP, IDX::SLIP); // set shape object.shape.dimensions.x = bounding_box_.length; 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 69d7e9b19ae88..f2a8488ec6089 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 @@ -53,7 +53,7 @@ NormalVehicleTracker::NormalVehicleTracker( float q_stddev_y = 1.0; // object coordinate [m/s] float q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // map coordinate[rad/s] float q_stddev_vx = tier4_autoware_utils::kmph2mps(10); // object coordinate [m/(s*s)] - float q_stddev_wz = tier4_autoware_utils::deg2rad(20); // object coordinate [rad/(s*s)] + float q_stddev_slip = tier4_autoware_utils::deg2rad(20); // object coordinate [rad/(s*s)] float r_stddev_x = 1.0; // object coordinate [m] float r_stddev_y = 0.3; // object coordinate [m] float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // map coordinate [rad] @@ -62,12 +62,12 @@ NormalVehicleTracker::NormalVehicleTracker( float p0_stddev_y = 0.3; // object coordinate [m/s] float p0_stddev_yaw = tier4_autoware_utils::deg2rad(30); // map coordinate [rad] float p0_stddev_vx = tier4_autoware_utils::kmph2mps(1000); // object coordinate [m/s] - float p0_stddev_wz = tier4_autoware_utils::deg2rad(10); // object coordinate [rad/s] + float p0_stddev_slip = tier4_autoware_utils::deg2rad(10); // object coordinate [rad/s] ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0); ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0); ekf_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0); ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); - ekf_params_.q_cov_wz = std::pow(q_stddev_wz, 2.0); + ekf_params_.q_cov_slip = std::pow(q_stddev_slip, 2.0); ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); @@ -76,9 +76,9 @@ NormalVehicleTracker::NormalVehicleTracker( ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0); ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0); - ekf_params_.p0_cov_wz = std::pow(p0_stddev_wz, 2.0); + ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); max_vx_ = tier4_autoware_utils::kmph2mps(100); // [m/s] - max_wz_ = tier4_autoware_utils::deg2rad(30); // [rad/s] + max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad/s] velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] // initialize X matrix @@ -88,11 +88,10 @@ NormalVehicleTracker::NormalVehicleTracker( X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); if (object.kinematics.has_twist) { X(IDX::VX) = object.kinematics.twist_with_covariance.twist.linear.x; - X(IDX::WZ) = object.kinematics.twist_with_covariance.twist.angular.z; } else { X(IDX::VX) = 0.0; - X(IDX::WZ) = 0.0; } + X(IDX::SLIP) = 0.0; // initialize P matrix Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); @@ -110,7 +109,7 @@ NormalVehicleTracker::NormalVehicleTracker( P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y); P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw; P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; - P(IDX::WZ, IDX::WZ) = ekf_params_.p0_cov_wz; + P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } else { P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; P(IDX::X, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; @@ -121,12 +120,10 @@ NormalVehicleTracker::NormalVehicleTracker( if (object.kinematics.has_twist_covariance) { P(IDX::VX, IDX::VX) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - P(IDX::WZ, IDX::WZ) = - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; } else { P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; - P(IDX::WZ, IDX::WZ) = ekf_params_.p0_cov_wz; } + P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { @@ -150,6 +147,11 @@ NormalVehicleTracker::NormalVehicleTracker( /* calc nearest corner index*/ setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step + + // Set lf, lr + double point_ratio = 0.5; // comes to front if larger + lf_ = bounding_box_.length * point_ratio; + lr_ = bounding_box_.length * (1.0 - point_ratio); } bool NormalVehicleTracker::predict(const rclcpp::Time & time) @@ -164,47 +166,53 @@ bool NormalVehicleTracker::predict(const rclcpp::Time & time) bool NormalVehicleTracker::predict(const double dt, KalmanFilter & ekf) const { - /* == Nonlinear model == + /* == Nonlinear model == static bicycle model * - * x_{k+1} = x_k + vx_k * cos(yaw_k) * dt - * y_{k+1} = y_k + vx_k * sin(yaw_k) * dt - * yaw_{k+1} = yaw_k + (wz_k) * dt + * x_{k+1} = x_k + vx_k * cos(yaw_k + slip_k) * dt + * y_{k+1} = y_k + vx_k * sin(yaw_k + slip_k) * dt + * yaw_{k+1} = yaw_k + vx_k / l_r * sin(slip_k) * dt * vx_{k+1} = vx_k - * wz_{k+1} = wz_k + * slip_{k+1} = slip_k * */ /* == Linearized model == * - * A = [ 1, 0, -vx*sin(yaw)*dt, cos(yaw)*dt, 0] - * [ 0, 1, vx*cos(yaw)*dt, sin(yaw)*dt, 0] - * [ 0, 0, 1, 0, dt] - * [ 0, 0, 0, 1, 0] - * [ 0, 0, 0, 0, 1] + * A = [ 1, 0, -vx*sin(yaw+slip)*dt, cos(yaw+slip)*dt, -vx*sin(yaw+slip)*dt] + * [ 0, 1, vx*cos(yaw+slip)*dt, sin(yaw+slip)*dt, vx*cos(yaw+slip)*dt] + * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vx/l_r*cos(slip)*dt] + * [ 0, 0, 0, 1, 0] + * [ 0, 0, 0, 0, 1] */ // X t Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state ekf.getX(X_t); - const double cos_yaw = std::cos(X_t(IDX::YAW)); - const double sin_yaw = std::sin(X_t(IDX::YAW)); + const double cos_yaw = std::cos(X_t(IDX::YAW) + X_t(IDX::SLIP)); + const double sin_yaw = std::sin(X_t(IDX::YAW) + X_t(IDX::SLIP)); + const double cos_slip = std::cos(X_t(IDX::SLIP)); + const double sin_slip = std::sin(X_t(IDX::SLIP)); + const double vx = X_t(IDX::VX); const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); // X t+1 - Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state - X_next_t(IDX::X) = X_t(IDX::X) + X_t(IDX::VX) * cos_yaw * dt; // dx = v * cos(yaw) - X_next_t(IDX::Y) = X_t(IDX::Y) + X_t(IDX::VX) * sin_yaw * dt; // dy = v * sin(yaw) - X_next_t(IDX::YAW) = X_t(IDX::YAW) + (X_t(IDX::WZ)) * dt; // dyaw = omega + Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state + X_next_t(IDX::X) = X_t(IDX::X) + vx * cos_yaw * dt; // dx = v * cos(yaw) + X_next_t(IDX::Y) = X_t(IDX::Y) + vx * sin_yaw * dt; // dy = v * sin(yaw) + X_next_t(IDX::YAW) = X_t(IDX::YAW) + vx / lr_ * sin_slip * dt; // dyaw = omega X_next_t(IDX::VX) = X_t(IDX::VX); - X_next_t(IDX::WZ) = X_t(IDX::WZ); + X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // A Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); - A(IDX::X, IDX::YAW) = -X_t(IDX::VX) * sin_yaw * dt; + A(IDX::X, IDX::YAW) = -vx * sin_yaw * dt; A(IDX::X, IDX::VX) = cos_yaw * dt; - A(IDX::Y, IDX::YAW) = X_t(IDX::VX) * cos_yaw * dt; + A(IDX::X, IDX::SLIP) = -vx * sin_yaw * dt; + A(IDX::Y, IDX::YAW) = vx * cos_yaw * dt; A(IDX::Y, IDX::VX) = sin_yaw * dt; - A(IDX::YAW, IDX::WZ) = dt; + A(IDX::Y, IDX::SLIP) = vx * cos_yaw * dt; + A(IDX::YAW, IDX::VX) = 1.0 / lr_ * sin_slip * dt; + A(IDX::YAW, IDX::SLIP) = vx / lr_ * cos_slip * dt; // Q Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); @@ -218,7 +226,7 @@ bool NormalVehicleTracker::predict(const double dt, KalmanFilter & ekf) const Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); Q(IDX::YAW, IDX::YAW) = ekf_params_.q_cov_yaw * dt * dt; Q(IDX::VX, IDX::VX) = ekf_params_.q_cov_vx * dt * dt; - Q(IDX::WZ, IDX::WZ) = ekf_params_.q_cov_wz * dt * dt; + Q(IDX::SLIP, IDX::SLIP) = ekf_params_.q_cov_slip * dt * dt; Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); @@ -356,8 +364,8 @@ bool NormalVehicleTracker::measureWithPose( if (!(-max_vx_ <= X_t(IDX::VX) && X_t(IDX::VX) <= max_vx_)) { X_t(IDX::VX) = X_t(IDX::VX) < 0 ? -max_vx_ : max_vx_; } - if (!(-max_wz_ <= X_t(IDX::WZ) && X_t(IDX::WZ) <= max_wz_)) { - X_t(IDX::WZ) = X_t(IDX::WZ) < 0 ? -max_wz_ : max_wz_; + if (!(-max_slip_ <= X_t(IDX::SLIP) && X_t(IDX::SLIP) <= max_slip_)) { + X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -max_slip_ : max_slip_; } ekf_.init(X_t, P_t); } @@ -486,7 +494,8 @@ bool NormalVehicleTracker::getTrackedObject( // twist twist_with_cov.twist.linear.x = X_t(IDX::VX); - twist_with_cov.twist.angular.z = X_t(IDX::WZ); + twist_with_cov.twist.angular.z = + X_t(IDX::VX) / lr_ * std::sin(X_t(IDX::SLIP)); // yaw_rate = vx_k / l_r * sin(slip_k) // twist covariance constexpr double vy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative @@ -495,11 +504,11 @@ bool NormalVehicleTracker::getTrackedObject( twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VX, IDX::VX); twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = vy_cov; twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = P(IDX::VX, IDX::WZ); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = P(IDX::WZ, IDX::VX); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = P(IDX::VX, IDX::SLIP); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = P(IDX::SLIP, IDX::VX); twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::WZ, IDX::WZ); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::SLIP, IDX::SLIP); // set shape object.shape.dimensions.x = bounding_box_.length;