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 ad268283d5890..6b4bfe51beade 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,16 +32,18 @@ class BicycleTracker : public Tracker private: KalmanFilter ekf_; rclcpp::Time last_update_time_; - enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, SLIP = 4 }; + enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, AX = 4, SLIP = 5 }; struct EkfParams { - char dim_x = 5; + char dim_x = 6; float q_cov_x; float q_cov_y; float q_cov_yaw; float q_cov_slip; float q_cov_vx; + float q_cov_ax; float p0_cov_vx; + float p0_cov_ax; float p0_cov_slip; float r_cov_x; float r_cov_y; @@ -52,6 +54,7 @@ class BicycleTracker : public Tracker } ekf_params_; double max_vx_; + double max_ax_; double max_slip_; double lf_; double lr_; 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 8a9b6adfc9cd5..7be355cc36e06 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,16 +32,18 @@ class BigVehicleTracker : public Tracker private: KalmanFilter ekf_; rclcpp::Time last_update_time_; - enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, SLIP = 4 }; + enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, AX = 4, SLIP = 5 }; struct EkfParams { - char dim_x = 5; + char dim_x = 6; float q_cov_x; float q_cov_y; float q_cov_yaw; float q_cov_slip; float q_cov_vx; + float q_cov_ax; float p0_cov_vx; + float p0_cov_ax; float p0_cov_slip; float r_cov_x; float r_cov_y; @@ -52,6 +54,7 @@ class BigVehicleTracker : public Tracker float p0_cov_yaw; } ekf_params_; double max_vx_; + double max_ax_; double max_slip_; double lf_; double lr_; 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 79e9a20a10421..d50032ea97bf4 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,17 +33,19 @@ class NormalVehicleTracker : public Tracker private: KalmanFilter ekf_; rclcpp::Time last_update_time_; - enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, SLIP = 4 }; + enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, AX = 4, SLIP = 5 }; struct EkfParams { - char dim_x = 5; + char dim_x = 6; float q_cov_x; float q_cov_y; float q_cov_yaw; float q_cov_slip; float q_cov_vx; + float q_cov_ax; float p0_cov_vx; + float p0_cov_ax; float p0_cov_slip; float r_cov_x; float r_cov_y; @@ -55,6 +57,7 @@ class NormalVehicleTracker : public Tracker } ekf_params_; double max_vx_; + double max_ax_; double max_slip_; double lf_; double lr_; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp index 0219d3b227044..8bc86496ff876 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp @@ -37,17 +37,20 @@ class PedestrianTracker : public Tracker Y = 1, YAW = 2, VX = 3, - WZ = 4, + AX = 4, + WZ = 5, }; struct EkfParams { - char dim_x = 5; + char dim_x = 6; float q_cov_x; float q_cov_y; float q_cov_yaw; float q_cov_wz; float q_cov_vx; + float q_cov_ax; float p0_cov_vx; + float p0_cov_ax; float p0_cov_wz; float r_cov_x; float r_cov_y; @@ -58,6 +61,7 @@ class PedestrianTracker : public Tracker } ekf_params_; double max_vx_; + double max_ax_; double max_wz_; float z_; 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 6571e70b8c123..0c503b6d6dfbb 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -57,6 +57,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_ax = 9.8 / 2; // [m/(s*s)] float q_stddev_slip = tier4_autoware_utils::deg2rad(15); // [rad/(s*s)] float r_stddev_x = 0.6; // [m] float r_stddev_y = 0.4; // [m] @@ -65,11 +66,13 @@ 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_ax = tier4_autoware_utils::kmph2mps(1000); // [m/(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_ax = std::pow(q_stddev_ax, 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); @@ -78,7 +81,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_ax = std::pow(p0_stddev_ax, 2.0); ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); + max_ax_ = 9.8 * 0.2; max_vx_ = tier4_autoware_utils::kmph2mps(100); // [m/s] max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad/s] @@ -93,6 +98,7 @@ BicycleTracker::BicycleTracker( X(IDX::VX) = 0.0; } X(IDX::SLIP) = 0.0; + X(IDX::AX) = 0.0; // initialize P matrix Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); @@ -111,6 +117,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::AX, IDX::AX) = ekf_params_.p0_cov_ax; 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]; @@ -126,6 +133,7 @@ BicycleTracker::BicycleTracker( P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; } P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; + P(IDX::AX, IDX::AX) = ekf_params_.p0_cov_ax; } if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { @@ -159,18 +167,20 @@ bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const * 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 + * vx_{k+1} = vx_k + ax_k * dt + * ax_{k+1} = choose_abs_min(ea * ax_k, -vx_k/dt) * slip_{k+1} = slip_k * */ /* == Linearized model == * - * 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] + * A = [ 1, 0, -vx*sin(yaw+slip)*dt, cos(yaw+slip)*dt, 0, -vx*sin(yaw+slip)*dt] + * [ 0, 1, vx*cos(yaw+slip)*dt, sin(yaw+slip)*dt, 0, vx*cos(yaw+slip)*dt] + * [ 0, 0, 1, 1/l_r*sin(slip)*dt, 0, vx/l_r*cos(slip)*dt] + * [ 0, 0, 0, 1, dt, 0] + * [ 0, 0, 0, 0, 1, 0] + * [ 0, 0, 0, 0, 0, 1] */ // X t @@ -182,14 +192,22 @@ bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const 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)); + const double acc_attenuate_rate = 0.8; // X t+1 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::VX) = X_t(IDX::VX) + X_t(IDX::AX) * dt; X_next_t(IDX::SLIP) = X_t(IDX::SLIP); + const double a1 = acc_attenuate_rate * X_t(IDX::AX); + const double a2 = -X_t(IDX::VX) / dt; + if (std::fabs(a1) > std::fabs(a2)) { + X_next_t(IDX::AX) = a2; + } else { + X_next_t(IDX::AX) = a1; + } // A Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); @@ -201,6 +219,13 @@ bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const 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; + A(IDX::VX, IDX::AX) = dt; + if (std::fabs(a1) > std::fabs(a2)) { + A(IDX::AX, IDX::AX) = 0; + A(IDX::AX, IDX::VX) = -1.0 / dt; + } else { + A(IDX::AX, IDX::AX) = acc_attenuate_rate; + } // Q Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); @@ -214,6 +239,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::AX, IDX::AX) = ekf_params_.q_cov_ax * 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); @@ -325,6 +351,9 @@ 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_ax_ <= X_t(IDX::AX) && X_t(IDX::AX) <= max_ax_)) { + X_t(IDX::AX) = X_t(IDX::AX) < 0 ? -max_ax_ : max_ax_; + } 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_; } @@ -395,6 +424,7 @@ bool BicycleTracker::getTrackedObject( auto & pose_with_cov = object.kinematics.pose_with_covariance; auto & twist_with_cov = object.kinematics.twist_with_covariance; + auto & acc_with_cov = object.kinematics.acceleration_with_covariance; // position pose_with_cov.pose.position.x = X_t(IDX::X); @@ -447,6 +477,11 @@ bool BicycleTracker::getTrackedObject( twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::SLIP, IDX::SLIP); + // acc + acc_with_cov.accel.linear.x = X_t(IDX::AX); + acc_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::AX, IDX::AX) * std::cos(X_t(IDX::SLIP)); + acc_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P(IDX::AX, IDX::AX) * std::sin(X_t(IDX::SLIP)); + // set shape object.shape.dimensions.x = bounding_box_.length; object.shape.dimensions.y = bounding_box_.width; 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 97d6d48c35d1b..82b05b5209a86 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 @@ -56,6 +56,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_ax = 9.8 * 0.2; // [m/(s*s)] float q_stddev_slip = tier4_autoware_utils::deg2rad(5); // [rad/(s*s)] float r_stddev_x = 1.5; // [m] float r_stddev_y = 0.5; // [m] @@ -65,11 +66,13 @@ 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_ax = tier4_autoware_utils::kmph2mps(1000); // [m/(s*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_ax = std::pow(q_stddev_ax, 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); @@ -79,8 +82,10 @@ 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_ax = std::pow(p0_stddev_ax, 2.0); ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); max_vx_ = tier4_autoware_utils::kmph2mps(100); // [m/s] + max_ax_ = 9.8 * 0.4; // [m/s^2] max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad] velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] @@ -96,6 +101,7 @@ BigVehicleTracker::BigVehicleTracker( } else { X(IDX::VX) = 0.0; } + X(IDX::AX) = 0.0; // initialize P matrix Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); @@ -113,6 +119,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::AX, IDX::AX) = ekf_params_.p0_cov_ax; 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]; @@ -128,6 +135,7 @@ BigVehicleTracker::BigVehicleTracker( P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; } P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; + P(IDX::AX, IDX::AX) = ekf_params_.p0_cov_ax; } if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { @@ -174,18 +182,20 @@ bool BigVehicleTracker::predict(const double dt, KalmanFilter & ekf) const * 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 + * vx_{k+1} = vx_k + ax_k * dt + * ax_{k+1} = ax_k * slip_{k+1} = slip_k * */ /* == Linearized model == * - * 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] + * A = [ 1, 0, -vx*sin(yaw+slip)*dt, cos(yaw+slip)*dt, 0, -vx*sin(yaw+slip)*dt] + * [ 0, 1, vx*cos(yaw+slip)*dt, sin(yaw+slip)*dt, 0, vx*cos(yaw+slip)*dt] + * [ 0, 0, 1, 1/l_r*sin(slip)*dt, 0, vx/l_r*cos(slip)*dt] + * [ 0, 0, 0, 1, dt, 0] + * [ 0, 0, 0, 0, 1, 0] + * [ 0, 0, 0, 0, 0, 1] */ // X t @@ -197,13 +207,15 @@ bool BigVehicleTracker::predict(const double dt, KalmanFilter & ekf) const 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)); + const double acc_attenuate_rate = 1.0; // X t+1 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::VX) = X_t(IDX::VX) + X_t(IDX::AX) * dt; + X_next_t(IDX::AX) = acc_attenuate_rate * X_t(IDX::AX); X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // A @@ -216,6 +228,8 @@ bool BigVehicleTracker::predict(const double dt, KalmanFilter & ekf) const 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; + A(IDX::VX, IDX::AX) = dt; + A(IDX::AX, IDX::AX) = acc_attenuate_rate; // Q Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); @@ -229,6 +243,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::AX, IDX::AX) = ekf_params_.q_cov_ax * 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,6 +371,9 @@ 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_ax_ <= X_t(IDX::AX) && X_t(IDX::AX) <= max_ax_)) { + X_t(IDX::AX) = X_t(IDX::AX) < 0 ? -max_ax_ : max_ax_; + } 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_; } @@ -438,6 +456,7 @@ bool BigVehicleTracker::getTrackedObject( /* put predicted pose and twist to output object */ auto & pose_with_cov = object.kinematics.pose_with_covariance; auto & twist_with_cov = object.kinematics.twist_with_covariance; + auto & acc_with_cov = object.kinematics.acceleration_with_covariance; // recover bounding box from tracking point const double dl = bounding_box_.length - last_input_bounding_box_.length; @@ -498,6 +517,11 @@ bool BigVehicleTracker::getTrackedObject( twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::SLIP, IDX::SLIP); + // acc + acc_with_cov.accel.linear.x = X_t(IDX::AX); + acc_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::AX, IDX::AX) * std::cos(X_t(IDX::SLIP)); + acc_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P(IDX::AX, IDX::AX) * std::sin(X_t(IDX::SLIP)); + // set shape object.shape.dimensions.x = bounding_box_.length; object.shape.dimensions.y = bounding_box_.width; 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 aa3f7b1c30d01..08ae2e0661d94 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 @@ -56,6 +56,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_ax = 9.8 * 0.2; // [m/(s*s)] float q_stddev_slip = tier4_autoware_utils::deg2rad(5); // object coordinate [rad/(s*s)] float r_stddev_x = 1.0; // object coordinate [m] float r_stddev_y = 0.3; // object coordinate [m] @@ -65,11 +66,13 @@ 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_ax = tier4_autoware_utils::kmph2mps(1000); // [m/(s*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_ax = std::pow(q_stddev_ax, 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); @@ -79,8 +82,10 @@ 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_ax = std::pow(p0_stddev_ax, 2.0); ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); max_vx_ = tier4_autoware_utils::kmph2mps(100); // [m/s] + max_ax_ = 9.8 * 0.4; // [m/s^2] max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad/s] velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] @@ -95,6 +100,7 @@ NormalVehicleTracker::NormalVehicleTracker( X(IDX::VX) = 0.0; } X(IDX::SLIP) = 0.0; + X(IDX::AX) = 0.0; // initialize P matrix Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); @@ -113,6 +119,7 @@ NormalVehicleTracker::NormalVehicleTracker( P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw; P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; + P(IDX::AX, IDX::AX) = ekf_params_.p0_cov_ax; } 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]; @@ -127,6 +134,7 @@ NormalVehicleTracker::NormalVehicleTracker( P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; } P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; + P(IDX::AX, IDX::AX) = ekf_params_.p0_cov_ax; } if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { @@ -174,18 +182,20 @@ bool NormalVehicleTracker::predict(const double dt, KalmanFilter & ekf) const * 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 + * vx_{k+1} = vx_k + ax_k * dt + * ax_{k+1} = ax_k * slip_{k+1} = slip_k * */ /* == Linearized model == * - * 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] + * A = [ 1, 0, -vx*sin(yaw+slip)*dt, cos(yaw+slip)*dt, 0, -vx*sin(yaw+slip)*dt] + * [ 0, 1, vx*cos(yaw+slip)*dt, sin(yaw+slip)*dt, 0, vx*cos(yaw+slip)*dt] + * [ 0, 0, 1, 1/l_r*sin(slip)*dt, 0, vx/l_r*cos(slip)*dt] + * [ 0, 0, 0, 1, dt, 0] + * [ 0, 0, 0, 0, 1, 0] + * [ 0, 0, 0, 0, 0, 1] */ // X t @@ -197,13 +207,15 @@ bool NormalVehicleTracker::predict(const double dt, KalmanFilter & ekf) const 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)); + const double acc_attenuate_rate = 1.0; // X t+1 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::VX) = X_t(IDX::VX) + X_t(IDX::AX) * dt; + X_next_t(IDX::AX) = acc_attenuate_rate * X_t(IDX::AX); X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // A @@ -216,6 +228,8 @@ bool NormalVehicleTracker::predict(const double dt, KalmanFilter & ekf) const 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; + A(IDX::VX, IDX::AX) = dt; + A(IDX::AX, IDX::AX) = acc_attenuate_rate; // Q Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); @@ -229,6 +243,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::AX, IDX::AX) = ekf_params_.q_cov_ax * 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); @@ -367,6 +382,9 @@ 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_ax_ <= X_t(IDX::AX) && X_t(IDX::AX) <= max_ax_)) { + X_t(IDX::AX) = X_t(IDX::AX) < 0 ? -max_ax_ : max_ax_; + } 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_; } @@ -454,6 +472,7 @@ bool NormalVehicleTracker::getTrackedObject( auto & pose_with_cov = object.kinematics.pose_with_covariance; auto & twist_with_cov = object.kinematics.twist_with_covariance; + auto & acc_with_cov = object.kinematics.acceleration_with_covariance; // recover bounding box from tracking point const double dl = bounding_box_.length - last_input_bounding_box_.length; @@ -514,6 +533,11 @@ bool NormalVehicleTracker::getTrackedObject( twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::SLIP, IDX::SLIP); + // acc + acc_with_cov.accel.linear.x = X_t(IDX::AX); + acc_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::AX, IDX::AX) * std::cos(X_t(IDX::SLIP)); + acc_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P(IDX::AX, IDX::AX) * std::sin(X_t(IDX::SLIP)); + // set shape object.shape.dimensions.x = bounding_box_.length; object.shape.dimensions.y = bounding_box_.width; 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 b168717042db3..d5377061026ee 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -57,6 +57,7 @@ PedestrianTracker::PedestrianTracker( float q_stddev_y = 0.4; // [m/s] float q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s] float q_stddev_vx = tier4_autoware_utils::kmph2mps(5); // [m/(s*s)] + float q_stddev_ax = 9.8 * 0.1; // [m/(s*s)] float q_stddev_wz = tier4_autoware_utils::deg2rad(20); // [rad/(s*s)] float r_stddev_x = 0.4; // [m] float r_stddev_y = 0.4; // [m] @@ -65,11 +66,13 @@ PedestrianTracker::PedestrianTracker( float p0_stddev_y = 1.0; // [m/s] float p0_stddev_yaw = tier4_autoware_utils::deg2rad(1000); // [rad/s] float p0_stddev_vx = tier4_autoware_utils::kmph2mps(5); // [m/(s*s)] + float p0_stddev_ax = 9.8 * 0.1; // [m/(s*s)] float p0_stddev_wz = 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_ax = std::pow(q_stddev_ax, 2.0); ekf_params_.q_cov_wz = std::pow(q_stddev_wz, 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); @@ -78,9 +81,11 @@ PedestrianTracker::PedestrianTracker( 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_ax = std::pow(p0_stddev_ax, 2.0); ekf_params_.p0_cov_wz = std::pow(p0_stddev_wz, 2.0); max_vx_ = tier4_autoware_utils::kmph2mps(10); // [m/s] - max_wz_ = tier4_autoware_utils::deg2rad(30); // [rad/s] + max_ax_ = 9.8 * 0.5; + max_wz_ = tier4_autoware_utils::deg2rad(30); // [rad/s] // initialize X matrix Eigen::MatrixXd X(ekf_params_.dim_x, 1); @@ -94,6 +99,7 @@ PedestrianTracker::PedestrianTracker( X(IDX::VX) = 0.0; X(IDX::WZ) = 0.0; } + X(IDX::AX) = 0.0; // initialize P matrix Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); @@ -128,6 +134,7 @@ PedestrianTracker::PedestrianTracker( P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; P(IDX::WZ, IDX::WZ) = ekf_params_.p0_cov_wz; } + P(IDX::AX, IDX::AX) = ekf_params_.p0_cov_ax; } bounding_box_ = {0.5, 0.5, 1.7}; @@ -159,18 +166,20 @@ bool PedestrianTracker::predict(const double dt, KalmanFilter & ekf) const * 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 - * vx_{k+1} = vx_k + * vx_{k+1} = vx_k + ax_k * dt + * ax_{k+1} = ax_k * wz_{k+1} = wz_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)*dt, cos(yaw)*dt, 0, 0] + * [ 0, 1, vx*cos(yaw)*dt, sin(yaw)*dt, 0, 0] + * [ 0, 0, 1, 0, 0, dt] + * [ 0, 0, 0, 1, dt, 0] + * [ 0, 0, 0, 0, 1, 0] + * [ 0, 0, 0, 0, 0, 1] */ // X t @@ -185,8 +194,9 @@ bool PedestrianTracker::predict(const double dt, KalmanFilter & ekf) const 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 - X_next_t(IDX::VX) = X_t(IDX::VX); + X_next_t(IDX::VX) = X_t(IDX::VX) + X_t(IDX::AX) * dt; // dvx = a X_next_t(IDX::WZ) = X_t(IDX::WZ); + X_next_t(IDX::AX) = X_t(IDX::AX); // A Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); @@ -195,6 +205,7 @@ bool PedestrianTracker::predict(const double dt, KalmanFilter & ekf) const A(IDX::Y, IDX::YAW) = X_t(IDX::VX) * cos_yaw * dt; A(IDX::Y, IDX::VX) = sin_yaw * dt; A(IDX::YAW, IDX::WZ) = dt; + A(IDX::VX, IDX::AX) = dt; // Q Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); @@ -209,6 +220,7 @@ bool PedestrianTracker::predict(const double dt, KalmanFilter & ekf) const 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::AX, IDX::AX) = ekf_params_.q_cov_ax * 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); @@ -284,6 +296,9 @@ bool PedestrianTracker::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_ax_ <= X_t(IDX::AX) && X_t(IDX::AX) <= max_ax_)) { + X_t(IDX::AX) = X_t(IDX::AX) < 0 ? -max_ax_ : max_ax_; + } 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_; } @@ -358,6 +373,7 @@ bool PedestrianTracker::getTrackedObject( auto & pose_with_cov = object.kinematics.pose_with_covariance; auto & twist_with_cov = object.kinematics.twist_with_covariance; + auto & acc_with_cov = object.kinematics.acceleration_with_covariance; // position pose_with_cov.pose.position.x = X_t(IDX::X); @@ -408,6 +424,10 @@ bool PedestrianTracker::getTrackedObject( 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); + // acc + acc_with_cov.accel.linear.x = X_t(IDX::AX); + acc_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::AX, IDX::AX); + // set shape if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { object.shape.dimensions.x = bounding_box_.length;