From d270383757ac64f1453f75fa4f6dd43d87ef02b8 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 21 Feb 2024 09:19:00 +0900 Subject: [PATCH 1/4] feat(multi_object_tracker): mot bicycle model revision (#6082) * fix: bicycle model length on big vehicle Signed-off-by: Taekjin LEE * fix: rear axis position fix in all modalities Signed-off-by: Taekjin LEE * fix: repeat predict to limit dt Signed-off-by: Taekjin LEE * fix: implement repeating prediction to all modalities Signed-off-by: Taekjin LEE * fix: bicycle model revision Signed-off-by: Taekjin LEE * fix: fix centripetal acceleration, jacobian Signed-off-by: Taekjin LEE * style(pre-commit): autofix Signed-off-by: Taekjin LEE * chore: equation on description, velocity index fix Signed-off-by: Taekjin LEE * fix: markdown math Signed-off-by: Taekjin LEE * chore: refactor params Signed-off-by: Taekjin LEE * feat: model slip process rate Signed-off-by: Taekjin LEE * feat: yaw rate limits Signed-off-by: Taekjin LEE * chore: refactor vx to vel, follow actual definition Signed-off-by: Taekjin LEE * fix: acceleration uncertainty equation fix Signed-off-by: Taekjin LEE * fix: twist covariance matrix Signed-off-by: Taekjin LEE * chore: explanations of Kalman filter matrices Signed-off-by: Taekjin LEE * feat: slip angle process noise model revised Signed-off-by: Taekjin LEE * feat: parameter tuning Signed-off-by: Taekjin LEE * fix: use mahalanobis distance gate with larger threshold Signed-off-by: Taekjin LEE * chore: align tracker format Signed-off-by: Taekjin LEE * style(pre-commit): autofix --------- Signed-off-by: Taekjin LEE Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Yoshi Ri Co-authored-by: Kotaro Yoshimoto --- .../tracker/model/bicycle_tracker.hpp | 18 +- .../tracker/model/big_vehicle_tracker.hpp | 20 +- .../tracker/model/normal_vehicle_tracker.hpp | 21 +- perception/multi_object_tracker/models.md | 45 ++- .../src/data_association/data_association.cpp | 2 +- .../src/tracker/model/bicycle_tracker.cpp | 297 +++++++++----- .../src/tracker/model/big_vehicle_tracker.cpp | 329 ++++++++++------ .../tracker/model/normal_vehicle_tracker.cpp | 371 +++++++++++------- 8 files changed, 702 insertions(+), 401 deletions(-) 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..ba50933eddaec 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, VEL = 3, SLIP = 4 }; struct EkfParams { char dim_x = 5; - float q_cov_x; - float q_cov_y; - float q_cov_yaw; - float q_cov_slip; - float q_cov_vx; - float p0_cov_vx; + float q_stddev_acc_long; + float q_stddev_acc_lat; + float q_stddev_yaw_rate_min; + float q_stddev_yaw_rate_max; + float q_cov_slip_rate_min; + float q_cov_slip_rate_max; + float q_max_slip_angle; + float p0_cov_vel; float p0_cov_slip; float r_cov_x; float r_cov_y; @@ -51,7 +53,7 @@ class BicycleTracker : public Tracker float p0_cov_yaw; } ekf_params_; - double max_vx_; + double max_vel_; 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..e19b6382ad952 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,26 +32,28 @@ 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, VEL = 3, SLIP = 4 }; struct EkfParams { char dim_x = 5; - float q_cov_x; - float q_cov_y; - float q_cov_yaw; - float q_cov_slip; - float q_cov_vx; - float p0_cov_vx; + float q_stddev_acc_long; + float q_stddev_acc_lat; + float q_stddev_yaw_rate_min; + float q_stddev_yaw_rate_max; + float q_cov_slip_rate_min; + float q_cov_slip_rate_max; + float q_max_slip_angle; + float p0_cov_vel; float p0_cov_slip; float r_cov_x; float r_cov_y; float r_cov_yaw; - float r_cov_vx; + float r_cov_vel; float p0_cov_x; float p0_cov_y; float p0_cov_yaw; } ekf_params_; - double max_vx_; + double max_vel_; 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..05fa0a5ef01ba 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,28 +33,30 @@ 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, VEL = 3, SLIP = 4 }; struct EkfParams { char dim_x = 5; - float q_cov_x; - float q_cov_y; - float q_cov_yaw; - float q_cov_slip; - float q_cov_vx; - float p0_cov_vx; + float q_stddev_acc_long; + float q_stddev_acc_lat; + float q_stddev_yaw_rate_min; + float q_stddev_yaw_rate_max; + float q_cov_slip_rate_min; + float q_cov_slip_rate_max; + float q_max_slip_angle; + float p0_cov_vel; float p0_cov_slip; float r_cov_x; float r_cov_y; float r_cov_yaw; - float r_cov_vx; + float r_cov_vel; float p0_cov_x; float p0_cov_y; float p0_cov_yaw; } ekf_params_; - double max_vx_; + double max_vel_; double max_slip_; double lf_; double lr_; @@ -88,6 +90,7 @@ class NormalVehicleTracker : public Tracker const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const override; void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform); + double getMeasurementYaw(const autoware_auto_perception_msgs::msg::DetectedObject & object); virtual ~NormalVehicleTracker() {} }; diff --git a/perception/multi_object_tracker/models.md b/perception/multi_object_tracker/models.md index f74967943be32..f1f53c7398c0a 100644 --- a/perception/multi_object_tracker/models.md +++ b/perception/multi_object_tracker/models.md @@ -11,21 +11,21 @@ CTRV model is a model that assumes constant turn rate and velocity magnitude. - state transition equation $$ -\begin{align*} -x_{k+1} &= x_k + v_{x_k} \cos(\psi_k) \cdot dt \\ -y_{k+1} &= y_k + v_{x_k} \sin(\psi_k) \cdot dt \\ -\psi_{k+1} &= \psi_k + \dot{\psi}_k \cdot dt \\ -v_{x_{k+1}} &= v_{x_k} \\ -\dot{\psi}_{k+1} &= \dot{\psi}_k \\ -\end{align*} +\begin{aligned} +x_{k+1} & = x_{k} + v_{k} \cos(\psi_k) \cdot {d t} \\ +y_{k+1} & = y_{k} + v_{k} \sin(\psi_k) \cdot {d t} \\ +\psi_{k+1} & = \psi_k + \dot\psi_{k} \cdot {d t} \\ +v_{k+1} & = v_{k} \\ +\dot\psi_{k+1} & = \dot\psi_{k} \\ +\end{aligned} $$ - jacobian $$ A = \begin{bmatrix} -1 & 0 & -v_x \sin(\psi) \cdot dt & \cos(\psi) \cdot dt & 0 \\ -0 & 1 & v_x \cos(\psi) \cdot dt & \sin(\psi) \cdot dt & 0 \\ +1 & 0 & -v \sin(\psi) \cdot dt & \cos(\psi) \cdot dt & 0 \\ +0 & 1 & v \cos(\psi) \cdot dt & \sin(\psi) \cdot dt & 0 \\ 0 & 0 & 1 & 0 & dt \\ 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 1 \\ @@ -40,17 +40,20 @@ The merit of using this model is that it can prevent unintended yaw rotation whe ![kinematic_bicycle_model](image/kinematic_bicycle_model.png) - **state variable** - - pose( $x,y$ ), velocity( $v$ ), yaw( $\psi$ ), and slip angle ( $\beta$ ) - - $[x_{k} ,y_{k} , v_{k} , \psi_{k} , \beta_{k} ]^\mathrm{T}$ + - pose( $x,y$ ), yaw( $\psi$ ), velocity( $v$ ), and slip angle ( $\beta$ ) + - $[x_{k}, y_{k}, \psi_{k}, v_{k}, \beta_{k} ]^\mathrm{T}$ - **Prediction Equation** - $dt$: sampling time + - $w_{k} = \dot\psi_{k} = \frac{ v_{k} \sin \left( \beta_{k} \right) }{ l_r }$ : angular velocity $$ \begin{aligned} -x_{k+1} & =x_{k}+v_{k} \cos \left(\psi_{k}+\beta_{k}\right) d t \\ -y_{k+1} & =y_{k}+v_{k} \sin \left(\psi_{k}+\beta_{k}\right) d t \\ -v_{k+1} & =v_{k} \\ -\psi_{k+1} & =\psi_{k}+\frac{v_{k}}{l_{r}} \sin \beta_{k} d t \\ +x_{k+1} & = x_{k} + v_{k} \cos \left( \psi_{k}+\beta_{k} \right) {d t} + -\frac{1}{2} \left\lbrace w_k v_k \sin \left(\psi_{k}+\beta_{k} \right) \right\rbrace {d t}^2\\ +y_{k+1} & = y_{k} + v_{k} \sin \left( \psi_{k}+\beta_{k} \right) {d t} + +\frac{1}{2} \left\lbrace w_k v_k \cos \left(\psi_{k}+\beta_{k} \right) \right\rbrace {d t}^2\\ +\psi_{k+1} & =\psi_{k} + w_k {d t} \\ +v_{k+1} & = v_{k} \\ \beta_{k+1} & =\beta_{k} \end{aligned} $$ @@ -59,9 +62,15 @@ $$ $$ \frac{\partial f}{\partial \mathrm x}=\left[\begin{array}{ccccc} -1 & 0 & -v \sin (\psi+\beta) d t & v \cos (\psi+\beta) & -v \sin (\psi+\beta) d t \\ -0 & 1 & v \cos (\psi+\beta) d t & v \sin (\psi+\beta) & v \cos (\psi+\beta) d t \\ -0 & 0 & 1 & \frac{1}{l_r} \sin \beta d t & \frac{v}{l_r} \cos \beta d t \\ +1 & 0 + & v \cos (\psi+\beta) {d t} - \frac{1}{2} \left\lbrace w v \cos \left( \psi+\beta \right) \right\rbrace {d t}^2 + & \sin (\psi+\beta) {d t} - \left\lbrace w \sin \left( \psi+\beta \right) \right\rbrace {d t}^2 + & -v \sin (\psi+\beta) {d t} - \frac{v^2}{2l_r} \left\lbrace \cos(\beta)\sin(\psi+\beta)+\sin(\beta)\cos(\psi+\beta) \right\rbrace {d t}^2 \\ +0 & 1 + & v \sin (\psi+\beta) {d t} - \frac{1}{2} \left\lbrace w v \sin \left( \psi+\beta \right) \right\rbrace {d t}^2 + & \cos (\psi+\beta) {d t} + \left\lbrace w \cos \left( \psi+\beta \right) \right\rbrace {d t}^2 + & v \cos (\psi+\beta) {d t} + \frac{v^2}{2l_r} \left\lbrace \cos(\beta)\cos(\psi+\beta)-\sin(\beta)\sin(\psi+\beta) \right\rbrace {d t}^2 \\ +0 & 0 & 1 & \frac{1}{l_r} \sin \beta {d t} & \frac{v}{l_r} \cos \beta d t \\ 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 1 \end{array}\right] diff --git a/perception/multi_object_tracker/src/data_association/data_association.cpp b/perception/multi_object_tracker/src/data_association/data_association.cpp index 5e7cadc423216..f367e19c11f2a 100644 --- a/perception/multi_object_tracker/src/data_association/data_association.cpp +++ b/perception/multi_object_tracker/src/data_association/data_association.cpp @@ -201,7 +201,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( measurement_object.kinematics.pose_with_covariance.pose.position, tracked_object.kinematics.pose_with_covariance.pose.position, getXYCovariance(tracked_object.kinematics.pose_with_covariance)); - if (2.448 /*95%*/ <= mahalanobis_dist) passed_gate = false; + if (3.035 /*99%*/ <= mahalanobis_dist) passed_gate = false; } // 2d iou gate if (passed_gate) { 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..0b56b11451ad3 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -52,51 +52,57 @@ BicycleTracker::BicycleTracker( { object_ = object; - // initialize params - float q_stddev_x = 0.6; // [m/s] - 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_slip = tier4_autoware_utils::deg2rad(15); // [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] - float p0_stddev_x = 0.8; // [m/s] - 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_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_slip = std::pow(q_stddev_slip, 2.0); + // Initialize parameters + // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty + float r_stddev_x = 0.5; // in vehicle coordinate [m] + float r_stddev_y = 0.4; // in vehicle coordinate [m] + float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] 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); + // initial state covariance + float p0_stddev_x = 0.8; // [m] + float p0_stddev_y = 0.5; // [m] + float p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] + float p0_stddev_vel = tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] + float p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0); 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_vel = std::pow(p0_stddev_vel, 2.0); ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); - max_vx_ = tier4_autoware_utils::kmph2mps(100); // [m/s] - max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad/s] - - // initialize X matrix + // process noise covariance + ekf_params_.q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration + ekf_params_.q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration + ekf_params_.q_stddev_yaw_rate_min = + tier4_autoware_utils::deg2rad(5.0); // [rad/s] uncertain yaw change rate + ekf_params_.q_stddev_yaw_rate_max = + tier4_autoware_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate + float q_stddev_slip_rate_min = + tier4_autoware_utils::deg2rad(1); // [rad/s] uncertain slip angle change rate + float q_stddev_slip_rate_max = + tier4_autoware_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate + ekf_params_.q_cov_slip_rate_min = std::pow(q_stddev_slip_rate_min, 2.0); + ekf_params_.q_cov_slip_rate_max = std::pow(q_stddev_slip_rate_max, 2.0); + ekf_params_.q_max_slip_angle = tier4_autoware_utils::deg2rad(30); // [rad] max slip angle + // limitations + max_vel_ = tier4_autoware_utils::kmph2mps(100); // [m/s] + max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad/s] + + // initialize state vector X Eigen::MatrixXd X(ekf_params_.dim_x, 1); 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::VEL) = object.kinematics.twist_with_covariance.twist.linear.x; } else { - X(IDX::VX) = 0.0; + X(IDX::VEL) = 0.0; } - X(IDX::SLIP) = 0.0; - // initialize P matrix + // initialize state covariance matrix P Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - if (!object.kinematics.has_position_covariance) { const double cos_yaw = std::cos(X(IDX::YAW)); const double sin_yaw = std::sin(X(IDX::YAW)); @@ -110,7 +116,7 @@ BicycleTracker::BicycleTracker( ekf_params_.p0_cov_x * sin_yaw * sin_yaw + ekf_params_.p0_cov_y * cos_yaw * cos_yaw; 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::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; 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]; @@ -120,10 +126,10 @@ BicycleTracker::BicycleTracker( P(IDX::YAW, IDX::YAW) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; if (object.kinematics.has_twist_covariance) { - P(IDX::VX, IDX::VX) = + P(IDX::VEL, IDX::VEL) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } else { - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; + P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; } P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } @@ -137,15 +143,28 @@ BicycleTracker::BicycleTracker( ekf_.init(X, P); // Set lf, lr - double point_ratio = 0.2; // under steered if smaller than 0.5 - lf_ = bounding_box_.length * point_ratio; - lr_ = bounding_box_.length * (1.0 - point_ratio); + lf_ = bounding_box_.length * 0.3; // 30% front from the center + lr_ = bounding_box_.length * 0.3; // 30% rear from the center } bool BicycleTracker::predict(const rclcpp::Time & time) { const double dt = (time - last_update_time_).seconds(); - bool ret = predict(dt, ekf_); + if (dt < 0.0) { + RCLCPP_WARN(logger_, "dt is negative. (%f)", dt); + return false; + } + // if dt is too large, shorten dt and repeat prediction + const double dt_max = 0.11; + const uint32_t repeat = std::ceil(dt / dt_max); + const double dt_ = dt / repeat; + bool ret = false; + for (uint32_t i = 0; i < repeat; ++i) { + ret = predict(dt_, ekf_); + if (!ret) { + return false; + } + } if (ret) { last_update_time_ = time; } @@ -154,69 +173,116 @@ bool BicycleTracker::predict(const rclcpp::Time & time) bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const { - /* == Nonlinear model == static bicycle model + /* static bicycle model (constant slip angle, constant velocity) * - * 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 + * w_k = vel_k * sin(slip_k) / l_r + * x_{k+1} = x_k + vel_k*cos(yaw_k+slip_k)*dt - 0.5*w_k*vel_k*sin(yaw_k+slip_k)*dt*dt + * y_{k+1} = y_k + vel_k*sin(yaw_k+slip_k)*dt + 0.5*w_k*vel_k*cos(yaw_k+slip_k)*dt*dt + * yaw_{k+1} = yaw_k + w_k * dt + * vel_{k+1} = vel_k * slip_{k+1} = slip_k * */ - /* == Linearized model == + /* Jacobian Matrix + * + * A = [ 1, 0, -vel*sin(yaw+slip)*dt - 0.5*w_k*vel_k*cos(yaw+slip)*dt*dt, + * cos(yaw+slip)*dt - w*sin(yaw+slip)*dt*dt, + * -vel*sin(yaw+slip)*dt - 0.5*vel*vel/l_r*(cos(slip)sin(yaw+slip)+sin(slip)cos(yaw+slip))*dt*dt ] + * [ 0, 1, vel*cos(yaw+slip)*dt - 0.5*w_k*vel_k*sin(yaw+slip)*dt*dt, + * sin(yaw+slip)*dt + w*cos(yaw+slip)*dt*dt, + * vel*cos(yaw+slip)*dt + 0.5*vel*vel/l_r*(cos(slip)cos(yaw+slip)-sin(slip)sin(yaw+slip))*dt*dt ] + * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vel/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, -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 + // Current state vector 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) + X_t(IDX::SLIP)); const double sin_yaw = std::sin(X_t(IDX::YAW) + X_t(IDX::SLIP)); + const double vel = X_t(IDX::VEL); 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) + 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::SLIP) = X_t(IDX::SLIP); - // A + double w = vel * sin_slip / lr_; + const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); + const double w_dtdt = w * dt * dt; + const double vv_dtdt__lr = vel * vel * dt * dt / lr_; + + // Predict state vector X t+1 + Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state + X_next_t(IDX::X) = + X_t(IDX::X) + vel * cos_yaw * dt - 0.5 * vel * sin_slip * w_dtdt; // dx = v * cos(yaw) * dt + X_next_t(IDX::Y) = + X_t(IDX::Y) + vel * sin_yaw * dt + 0.5 * vel * cos_slip * w_dtdt; // dy = v * sin(yaw) * dt + X_next_t(IDX::YAW) = X_t(IDX::YAW) + w * dt; // d(yaw) = w * dt + X_next_t(IDX::VEL) = X_t(IDX::VEL); + X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // slip_angle = asin(lr * w / v) + + // State transition matrix A Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); - A(IDX::X, IDX::YAW) = -vx * sin_yaw * dt; - A(IDX::X, 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::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 + A(IDX::X, IDX::YAW) = -vel * sin_yaw * dt - 0.5 * vel * cos_yaw * w_dtdt; + A(IDX::X, IDX::VEL) = cos_yaw * dt - sin_yaw * w_dtdt; + A(IDX::X, IDX::SLIP) = + -vel * sin_yaw * dt - 0.5 * (cos_slip * sin_yaw + sin_slip * cos_yaw) * vv_dtdt__lr; + A(IDX::Y, IDX::YAW) = vel * cos_yaw * dt - 0.5 * vel * sin_yaw * w_dtdt; + A(IDX::Y, IDX::VEL) = sin_yaw * dt + cos_yaw * w_dtdt; + A(IDX::Y, IDX::SLIP) = + vel * cos_yaw * dt + 0.5 * (cos_slip * cos_yaw - sin_slip * sin_yaw) * vv_dtdt__lr; + A(IDX::YAW, IDX::VEL) = 1.0 / lr_ * sin_slip * dt; + A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt; + + // Process noise covariance Q + float q_stddev_yaw_rate{0.0}; + if (vel <= 0.01) { + q_stddev_yaw_rate = ekf_params_.q_stddev_yaw_rate_min; + } else { + // uncertainty of the yaw rate is limited by + // centripetal acceleration a_lat : d(yaw)/dt = w = a_lat/v + // or maximum slip angle slip_max : w = v*sin(slip_max)/l_r + q_stddev_yaw_rate = std::min( + ekf_params_.q_stddev_acc_lat / vel, + vel * std::sin(ekf_params_.q_max_slip_angle) / lr_); // [rad/s] + q_stddev_yaw_rate = std::min(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_max); + q_stddev_yaw_rate = std::max(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_min); + } + float q_cov_slip_rate{0.0f}; + if (vel <= 0.01) { + q_cov_slip_rate = ekf_params_.q_cov_slip_rate_min; + } else { + // The slip angle rate uncertainty is modeled as follows: + // d(slip)/dt ~ - sin(slip)/v * d(v)/dt + l_r/v * d(w)/dt + // where sin(slip) = w * l_r / v + // d(w)/dt is assumed to be proportional to w (more uncertain when slip is large) + // d(v)/dt and d(w)/t are considered to be uncorrelated + q_cov_slip_rate = + std::pow(ekf_params_.q_stddev_acc_lat * sin_slip / vel, 2) + std::pow(sin_slip * 1.5, 2); + q_cov_slip_rate = std::min(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_max); + q_cov_slip_rate = std::max(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_min); + } + const float q_cov_x = std::pow(0.5 * ekf_params_.q_stddev_acc_long * dt * dt, 2); + const float q_cov_y = std::pow(0.5 * ekf_params_.q_stddev_acc_lat * dt * dt, 2); + const float q_cov_yaw = std::pow(q_stddev_yaw_rate * dt, 2); + const float q_cov_vel = std::pow(ekf_params_.q_stddev_acc_long * dt, 2); + const float q_cov_slip = q_cov_slip_rate * dt * dt; + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); // Rotate the covariance matrix according to the vehicle yaw // because q_cov_x and y are in the vehicle coordinate system. - Q(IDX::X, IDX::X) = - (ekf_params_.q_cov_x * cos_yaw * cos_yaw + ekf_params_.q_cov_y * sin_yaw * sin_yaw) * dt * dt; - Q(IDX::X, IDX::Y) = (0.5f * (ekf_params_.q_cov_x - ekf_params_.q_cov_y) * sin_2yaw) * dt * dt; - Q(IDX::Y, IDX::Y) = - (ekf_params_.q_cov_x * sin_yaw * sin_yaw + ekf_params_.q_cov_y * cos_yaw * cos_yaw) * dt * dt; + Q(IDX::X, IDX::X) = (q_cov_x * cos_yaw * cos_yaw + q_cov_y * sin_yaw * sin_yaw); + Q(IDX::X, IDX::Y) = (0.5f * (q_cov_x - q_cov_y) * sin_2yaw); + Q(IDX::Y, IDX::Y) = (q_cov_x * sin_yaw * sin_yaw + q_cov_y * cos_yaw * cos_yaw); 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::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); + Q(IDX::YAW, IDX::YAW) = q_cov_yaw; + Q(IDX::VEL, IDX::VEL) = q_cov_vel; + Q(IDX::SLIP, IDX::SLIP) = q_cov_slip; + + // control-input model B and control-input u are not used + // 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); if (!ekf.predict(X_next_t, A, Q)) { RCLCPP_WARN(logger_, "Cannot predict"); @@ -310,20 +376,20 @@ bool BicycleTracker::measureWithPose( } } - // update with ekf + // ekf update if (!ekf_.update(Y, C, R)) { RCLCPP_WARN(logger_, "Cannot update"); } - // normalize yaw and limit vx, wz + // normalize yaw and limit vel, slip { Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); ekf_.getX(X_t); ekf_.getP(P_t); X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); - 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_vel_ <= X_t(IDX::VEL) && X_t(IDX::VEL) <= max_vel_)) { + X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -max_vel_ : max_vel_; } 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_; @@ -341,14 +407,26 @@ bool BicycleTracker::measureWithPose( bool BicycleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { + // if the input shape is convex type, convert it to bbox type + autoware_auto_perception_msgs::msg::DetectedObject bbox_object; if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - return false; + utils::convertConvexHullToBoundingBox(object, bbox_object); + } else { + bbox_object = object; } - constexpr float gain = 0.9; - bounding_box_.length = gain * bounding_box_.length + (1.0 - gain) * object.shape.dimensions.x; - bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * object.shape.dimensions.y; - bounding_box_.height = gain * bounding_box_.height + (1.0 - gain) * object.shape.dimensions.z; + constexpr float gain = 0.9; + bounding_box_.length = + gain * bounding_box_.length + (1.0 - gain) * bbox_object.shape.dimensions.x; + bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * bbox_object.shape.dimensions.y; + bounding_box_.height = + gain * bounding_box_.height + (1.0 - gain) * bbox_object.shape.dimensions.z; + last_input_bounding_box_ = { + bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; + + // update lf, lr + lf_ = bounding_box_.length * 0.3; // 30% front from the center + lr_ = bounding_box_.length * 0.3; // 30% rear from the center return true; } @@ -382,7 +460,7 @@ bool BicycleTracker::getTrackedObject( object.object_id = getUUID(); object.classification = getClassification(); - // predict kinematics + // predict state KalmanFilter tmp_ekf_for_no_update = ekf_; const double dt = (time - last_update_time_).seconds(); if (0.001 /*1msec*/ < dt) { @@ -429,23 +507,44 @@ bool BicycleTracker::getTrackedObject( pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); // twist - twist_with_cov.twist.linear.x = X_t(IDX::VX) * std::cos(X_t(IDX::SLIP)); - twist_with_cov.twist.linear.y = X_t(IDX::VX) * std::sin(X_t(IDX::SLIP)); + twist_with_cov.twist.linear.x = X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)); + twist_with_cov.twist.linear.y = X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)); 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 + X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)) / lr_; // yaw_rate = vel_k * sin(slip_k) / l_r + /* twist covariance + * convert covariance from velocity, slip angle to vx, vy, and yaw angle + * + * vx = vel * cos(slip) + * vy = vel * sin(slip) + * wz = vel * sin(slip) / l_r = vy / l_r + * + */ + constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - 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; + + Eigen::MatrixXd cov_jacob(3, 2); + cov_jacob << std::cos(X_t(IDX::SLIP)), -X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)), + std::sin(X_t(IDX::SLIP)), X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)), + std::sin(X_t(IDX::SLIP)) / lr_, X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)) / lr_; + Eigen::MatrixXd cov_twist(2, 2); + cov_twist << P(IDX::VEL, IDX::VEL), P(IDX::VEL, IDX::SLIP), P(IDX::SLIP, IDX::VEL), + P(IDX::SLIP, IDX::SLIP); + Eigen::MatrixXd twist_cov_mat = cov_jacob * cov_twist * cov_jacob.transpose(); + + twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = twist_cov_mat(0, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = twist_cov_mat(0, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = twist_cov_mat(0, 2); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = twist_cov_mat(1, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = twist_cov_mat(1, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_YAW] = twist_cov_mat(1, 2); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = twist_cov_mat(2, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_Y] = twist_cov_mat(2, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = twist_cov_mat(2, 2); 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::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::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 97d6d48c35d1b..34a72437d7721 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 @@ -16,6 +16,14 @@ // Author: v1.0 Yukihiro Saito // +#include "multi_object_tracker/tracker/model/big_vehicle_tracker.hpp" + +#include "multi_object_tracker/utils/utils.hpp" + +#include +#include +#include + #include #include #include @@ -26,17 +34,11 @@ #else #include #endif - -#define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/model/big_vehicle_tracker.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" +#define EIGEN_MPL2_ONLY #include #include -#include -#include -#include using Label = autoware_auto_perception_msgs::msg::ObjectClassification; @@ -51,53 +53,59 @@ BigVehicleTracker::BigVehicleTracker( { object_ = object; - // initialize params - float q_stddev_x = 1.5; // [m/s] - 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_slip = tier4_autoware_utils::deg2rad(5); // [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] - float r_stddev_vx = 1.0; // [m/s] - float p0_stddev_x = 1.5; // [m] - 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_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_slip = std::pow(q_stddev_slip, 2.0); + // Initialize parameters + // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty + float r_stddev_x = 0.5; // in vehicle coordinate [m] + float r_stddev_y = 0.4; // in vehicle coordinate [m] + float r_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad] + float r_stddev_vel = 1.0; // [m/s] 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); - ekf_params_.r_cov_vx = std::pow(r_stddev_vx, 2.0); + ekf_params_.r_cov_vel = std::pow(r_stddev_vel, 2.0); + // initial state covariance + float p0_stddev_x = 1.5; // [m] + float p0_stddev_y = 0.5; // [m] + float p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] + float p0_stddev_vel = tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] + float p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0); 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_vel = std::pow(p0_stddev_vel, 2.0); ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); - max_vx_ = tier4_autoware_utils::kmph2mps(100); // [m/s] + // process noise covariance + ekf_params_.q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration + ekf_params_.q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration + ekf_params_.q_stddev_yaw_rate_min = + tier4_autoware_utils::deg2rad(1.5); // [rad/s] uncertain yaw change rate + ekf_params_.q_stddev_yaw_rate_max = + tier4_autoware_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate + float q_stddev_slip_rate_min = + tier4_autoware_utils::deg2rad(0.3); // [rad/s] uncertain slip angle change rate + float q_stddev_slip_rate_max = + tier4_autoware_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate + ekf_params_.q_cov_slip_rate_min = std::pow(q_stddev_slip_rate_min, 2.0); + ekf_params_.q_cov_slip_rate_max = std::pow(q_stddev_slip_rate_max, 2.0); + ekf_params_.q_max_slip_angle = tier4_autoware_utils::deg2rad(30); // [rad] max slip angle + // limitations + max_vel_ = tier4_autoware_utils::kmph2mps(100); // [m/s] max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad] velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] - // initialize X matrix + // initialize state vector X Eigen::MatrixXd X(ekf_params_.dim_x, 1); 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::YAW) = object.kinematics.twist_with_covariance.twist.angular.z; + X(IDX::VEL) = object.kinematics.twist_with_covariance.twist.linear.x; } else { - X(IDX::VX) = 0.0; + X(IDX::VEL) = 0.0; } - // initialize P matrix + // initialize state covariance matrix P Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); if (!object.kinematics.has_position_covariance) { const double cos_yaw = std::cos(X(IDX::YAW)); @@ -112,7 +120,7 @@ BigVehicleTracker::BigVehicleTracker( ekf_params_.p0_cov_x * sin_yaw * sin_yaw + ekf_params_.p0_cov_y * cos_yaw * cos_yaw; 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::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; 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]; @@ -122,10 +130,10 @@ BigVehicleTracker::BigVehicleTracker( P(IDX::YAW, IDX::YAW) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; if (object.kinematics.has_twist_covariance) { - P(IDX::VX, IDX::VX) = + P(IDX::VEL, IDX::VEL) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } else { - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; + P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; } P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } @@ -142,9 +150,7 @@ BigVehicleTracker::BigVehicleTracker( bounding_box_ = { bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; - last_input_bounding_box_ = { - bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, - bbox_object.shape.dimensions.z}; + last_input_bounding_box_ = bounding_box_; } ekf_.init(X, P); @@ -152,15 +158,28 @@ BigVehicleTracker::BigVehicleTracker( setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step // Set lf, lr - double point_ratio = 0.2; // under steered if smaller than 0.5 - lf_ = bounding_box_.length * point_ratio; - lr_ = bounding_box_.length * (1.0 - point_ratio); + lf_ = bounding_box_.length * 0.3; // 30% front from the center + lr_ = bounding_box_.length * 0.25; // 25% rear from the center } bool BigVehicleTracker::predict(const rclcpp::Time & time) { const double dt = (time - last_update_time_).seconds(); - bool ret = predict(dt, ekf_); + if (dt < 0.0) { + RCLCPP_WARN(logger_, "dt is negative. (%f)", dt); + return false; + } + // if dt is too large, shorten dt and repeat prediction + const double dt_max = 0.11; + const uint32_t repeat = std::ceil(dt / dt_max); + const double dt_ = dt / repeat; + bool ret = false; + for (uint32_t i = 0; i < repeat; ++i) { + ret = predict(dt_, ekf_); + if (!ret) { + return false; + } + } if (ret) { last_update_time_ = time; } @@ -169,69 +188,116 @@ bool BigVehicleTracker::predict(const rclcpp::Time & time) bool BigVehicleTracker::predict(const double dt, KalmanFilter & ekf) const { - /* == Nonlinear model == static bicycle model + /* static bicycle model (constant slip angle, constant velocity) * - * 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 + * w_k = vel_k * sin(slip_k) / l_r + * x_{k+1} = x_k + vel_k*cos(yaw_k+slip_k)*dt - 0.5*w_k*vel_k*sin(yaw_k+slip_k)*dt*dt + * y_{k+1} = y_k + vel_k*sin(yaw_k+slip_k)*dt + 0.5*w_k*vel_k*cos(yaw_k+slip_k)*dt*dt + * yaw_{k+1} = yaw_k + w_k * dt + * vel_{k+1} = vel_k * slip_{k+1} = slip_k * */ - /* == Linearized model == + /* Jacobian Matrix + * + * A = [ 1, 0, -vel*sin(yaw+slip)*dt - 0.5*w_k*vel_k*cos(yaw+slip)*dt*dt, + * cos(yaw+slip)*dt - w*sin(yaw+slip)*dt*dt, + * -vel*sin(yaw+slip)*dt - 0.5*vel*vel/l_r*(cos(slip)sin(yaw+slip)+sin(slip)cos(yaw+slip))*dt*dt ] + * [ 0, 1, vel*cos(yaw+slip)*dt - 0.5*w_k*vel_k*sin(yaw+slip)*dt*dt, + * sin(yaw+slip)*dt + w*cos(yaw+slip)*dt*dt, + * vel*cos(yaw+slip)*dt + 0.5*vel*vel/l_r*(cos(slip)cos(yaw+slip)-sin(slip)sin(yaw+slip))*dt*dt ] + * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vel/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, -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 + // Current state vector 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) + X_t(IDX::SLIP)); const double sin_yaw = std::sin(X_t(IDX::YAW) + X_t(IDX::SLIP)); + const double vel = X_t(IDX::VEL); 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) + 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::SLIP) = X_t(IDX::SLIP); - // A + double w = vel * sin_slip / lr_; + const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); + const double w_dtdt = w * dt * dt; + const double vv_dtdt__lr = vel * vel * dt * dt / lr_; + + // Predict state vector X t+1 + Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state + X_next_t(IDX::X) = + X_t(IDX::X) + vel * cos_yaw * dt - 0.5 * vel * sin_slip * w_dtdt; // dx = v * cos(yaw) * dt + X_next_t(IDX::Y) = + X_t(IDX::Y) + vel * sin_yaw * dt + 0.5 * vel * cos_slip * w_dtdt; // dy = v * sin(yaw) * dt + X_next_t(IDX::YAW) = X_t(IDX::YAW) + w * dt; // d(yaw) = w * dt + X_next_t(IDX::VEL) = X_t(IDX::VEL); + X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // slip_angle = asin(lr * w / v) + + // State transition matrix A Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); - A(IDX::X, IDX::YAW) = -vx * sin_yaw * dt; - A(IDX::X, 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::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 + A(IDX::X, IDX::YAW) = -vel * sin_yaw * dt - 0.5 * vel * cos_yaw * w_dtdt; + A(IDX::X, IDX::VEL) = cos_yaw * dt - sin_yaw * w_dtdt; + A(IDX::X, IDX::SLIP) = + -vel * sin_yaw * dt - 0.5 * (cos_slip * sin_yaw + sin_slip * cos_yaw) * vv_dtdt__lr; + A(IDX::Y, IDX::YAW) = vel * cos_yaw * dt - 0.5 * vel * sin_yaw * w_dtdt; + A(IDX::Y, IDX::VEL) = sin_yaw * dt + cos_yaw * w_dtdt; + A(IDX::Y, IDX::SLIP) = + vel * cos_yaw * dt + 0.5 * (cos_slip * cos_yaw - sin_slip * sin_yaw) * vv_dtdt__lr; + A(IDX::YAW, IDX::VEL) = 1.0 / lr_ * sin_slip * dt; + A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt; + + // Process noise covariance Q + float q_stddev_yaw_rate{0.0}; + if (vel <= 0.01) { + q_stddev_yaw_rate = ekf_params_.q_stddev_yaw_rate_min; + } else { + // uncertainty of the yaw rate is limited by + // centripetal acceleration a_lat : d(yaw)/dt = w = a_lat/v + // or maximum slip angle slip_max : w = v*sin(slip_max)/l_r + q_stddev_yaw_rate = std::min( + ekf_params_.q_stddev_acc_lat / vel, + vel * std::sin(ekf_params_.q_max_slip_angle) / lr_); // [rad/s] + q_stddev_yaw_rate = std::min(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_max); + q_stddev_yaw_rate = std::max(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_min); + } + float q_cov_slip_rate{0.0f}; + if (vel <= 0.01) { + q_cov_slip_rate = ekf_params_.q_cov_slip_rate_min; + } else { + // The slip angle rate uncertainty is modeled as follows: + // d(slip)/dt ~ - sin(slip)/v * d(v)/dt + l_r/v * d(w)/dt + // where sin(slip) = w * l_r / v + // d(w)/dt is assumed to be proportional to w (more uncertain when slip is large) + // d(v)/dt and d(w)/t are considered to be uncorrelated + q_cov_slip_rate = + std::pow(ekf_params_.q_stddev_acc_lat * sin_slip / vel, 2) + std::pow(sin_slip * 1.5, 2); + q_cov_slip_rate = std::min(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_max); + q_cov_slip_rate = std::max(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_min); + } + const float q_cov_x = std::pow(0.5 * ekf_params_.q_stddev_acc_long * dt * dt, 2); + const float q_cov_y = std::pow(0.5 * ekf_params_.q_stddev_acc_lat * dt * dt, 2); + const float q_cov_yaw = std::pow(q_stddev_yaw_rate * dt, 2); + const float q_cov_vel = std::pow(ekf_params_.q_stddev_acc_long * dt, 2); + const float q_cov_slip = q_cov_slip_rate * dt * dt; + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); // Rotate the covariance matrix according to the vehicle yaw // because q_cov_x and y are in the vehicle coordinate system. - Q(IDX::X, IDX::X) = - (ekf_params_.q_cov_x * cos_yaw * cos_yaw + ekf_params_.q_cov_y * sin_yaw * sin_yaw) * dt * dt; - Q(IDX::X, IDX::Y) = (0.5f * (ekf_params_.q_cov_x - ekf_params_.q_cov_y) * sin_2yaw) * dt * dt; - Q(IDX::Y, IDX::Y) = - (ekf_params_.q_cov_x * sin_yaw * sin_yaw + ekf_params_.q_cov_y * cos_yaw * cos_yaw) * dt * dt; + Q(IDX::X, IDX::X) = (q_cov_x * cos_yaw * cos_yaw + q_cov_y * sin_yaw * sin_yaw); + Q(IDX::X, IDX::Y) = (0.5f * (q_cov_x - q_cov_y) * sin_2yaw); + Q(IDX::Y, IDX::Y) = (q_cov_x * sin_yaw * sin_yaw + q_cov_y * cos_yaw * cos_yaw); 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::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); + Q(IDX::YAW, IDX::YAW) = q_cov_yaw; + Q(IDX::VEL, IDX::VEL) = q_cov_vel; + Q(IDX::SLIP, IDX::SLIP) = q_cov_slip; + + // control-input model B and control-input u are not used + // 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); if (!ekf.predict(X_next_t, A, Q)) { RCLCPP_WARN(logger_, "Cannot predict"); @@ -249,14 +315,14 @@ bool BigVehicleTracker::measureWithPose( float r_cov_y; const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); - if (label == Label::CAR) { - constexpr float r_stddev_x = 8.0; // [m] - constexpr float r_stddev_y = 0.8; // [m] - r_cov_x = std::pow(r_stddev_x, 2.0); - r_cov_y = std::pow(r_stddev_y, 2.0); - } else if (utils::isLargeVehicleLabel(label)) { + if (utils::isLargeVehicleLabel(label)) { r_cov_x = ekf_params_.r_cov_x; r_cov_y = ekf_params_.r_cov_y; + } else if (label == Label::CAR) { + constexpr float r_stddev_x = 2.0; // [m] + constexpr float r_stddev_y = 2.0; // [m] + r_cov_x = std::pow(r_stddev_x, 2.0); + r_cov_y = std::pow(r_stddev_y, 2.0); } else { r_cov_x = ekf_params_.r_cov_x; r_cov_y = ekf_params_.r_cov_y; @@ -267,16 +333,16 @@ bool BigVehicleTracker::measureWithPose( if (object.kinematics.has_twist) { Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state ekf_.getX(X_t); - const double predicted_vx = X_t(IDX::VX); - const double observed_vx = object.kinematics.twist_with_covariance.twist.linear.x; + const double predicted_vel = X_t(IDX::VEL); + const double observed_vel = object.kinematics.twist_with_covariance.twist.linear.x; - if (std::fabs(predicted_vx - observed_vx) < velocity_deviation_threshold_) { + if (std::fabs(predicted_vel - observed_vel) < velocity_deviation_threshold_) { // Velocity deviation is small enable_velocity_measurement = true; } } - // pos x, pos y, yaw, vx depending on pose measurement + // pos x, pos y, yaw, vel depending on pose measurement const int dim_y = enable_velocity_measurement ? 4 : 3; double measurement_yaw = getMeasurementYaw(object); // get sign-solved yaw angle Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state @@ -296,7 +362,7 @@ bool BigVehicleTracker::measureWithPose( last_input_bounding_box_.width, last_input_bounding_box_.length, last_nearest_corner_index_, bbox_object, X_t(IDX::YAW), offset_object, tracking_offset_); - /* Set measurement matrix */ + /* Set measurement matrix and noise covariance*/ Eigen::MatrixXd Y(dim_y, 1); Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); @@ -330,31 +396,32 @@ bool BigVehicleTracker::measureWithPose( R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; } + // Update the velocity when necessary if (dim_y == 4) { - Y(IDX::VX, 0) = object.kinematics.twist_with_covariance.twist.linear.x; - C(3, IDX::VX) = 1.0; // for vx + Y(IDX::VEL, 0) = object.kinematics.twist_with_covariance.twist.linear.x; + C(3, IDX::VEL) = 1.0; // for vel if (!object.kinematics.has_twist_covariance) { - R(3, 3) = ekf_params_.r_cov_vx; // vx -vx + R(3, 3) = ekf_params_.r_cov_vel; // vel -vel } else { R(3, 3) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } } - /* ekf tracks constant tracking point */ + // ekf update if (!ekf_.update(Y, C, R)) { RCLCPP_WARN(logger_, "Cannot update"); } - // normalize yaw and limit vx, slip + // normalize yaw and limit vel, slip { Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); ekf_.getX(X_t); ekf_.getP(P_t); X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); - 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_vel_ <= X_t(IDX::VEL) && X_t(IDX::VEL) <= max_vel_)) { + X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -max_vel_ : max_vel_; } 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_; @@ -372,9 +439,8 @@ bool BigVehicleTracker::measureWithPose( bool BigVehicleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { + // if the input shape is convex type, convert it to bbox type autoware_auto_perception_msgs::msg::DetectedObject bbox_object; - - // if input is convex shape convert it to bbox shape if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { utils::convertConvexHullToBoundingBox(object, bbox_object); } else { @@ -389,6 +455,11 @@ bool BigVehicleTracker::measureWithShape( gain * bounding_box_.height + (1.0 - gain) * bbox_object.shape.dimensions.z; last_input_bounding_box_ = { bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; + + // update lf, lr + lf_ = bounding_box_.length * 0.3; // 30% front from the center + lr_ = bounding_box_.length * 0.25; // 25% rear from the center + return true; } @@ -480,23 +551,44 @@ bool BigVehicleTracker::getTrackedObject( pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); // twist - twist_with_cov.twist.linear.x = X_t(IDX::VX) * std::cos(X_t(IDX::SLIP)); - twist_with_cov.twist.linear.y = X_t(IDX::VX) * std::sin(X_t(IDX::SLIP)); + twist_with_cov.twist.linear.x = X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)); + twist_with_cov.twist.linear.y = X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)); 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 + X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)) / lr_; // yaw_rate = vel_k * sin(slip_k) / l_r + /* twist covariance + * convert covariance from velocity, slip angle to vx, vy, and yaw angle + * + * vx = vel * cos(slip) + * vy = vel * sin(slip) + * wz = vel * sin(slip) / l_r = vy / l_r + * + */ + constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - 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; + + Eigen::MatrixXd cov_jacob(3, 2); + cov_jacob << std::cos(X_t(IDX::SLIP)), -X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)), + std::sin(X_t(IDX::SLIP)), X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)), + std::sin(X_t(IDX::SLIP)) / lr_, X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)) / lr_; + Eigen::MatrixXd cov_twist(2, 2); + cov_twist << P(IDX::VEL, IDX::VEL), P(IDX::VEL, IDX::SLIP), P(IDX::SLIP, IDX::VEL), + P(IDX::SLIP, IDX::SLIP); + Eigen::MatrixXd twist_cov_mat = cov_jacob * cov_twist * cov_jacob.transpose(); + + twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = twist_cov_mat(0, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = twist_cov_mat(0, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = twist_cov_mat(0, 2); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = twist_cov_mat(1, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = twist_cov_mat(1, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_YAW] = twist_cov_mat(1, 2); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = twist_cov_mat(2, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_Y] = twist_cov_mat(2, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = twist_cov_mat(2, 2); 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::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::SLIP, IDX::SLIP); // set shape object.shape.dimensions.x = bounding_box_.length; @@ -506,7 +598,6 @@ bool BigVehicleTracker::getTrackedObject( const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); object.shape.footprint = tier4_autoware_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); - return true; } 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..fcf72360b84c2 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 @@ -16,6 +16,14 @@ // Author: v1.0 Yukihiro Saito // +#include "multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp" + +#include "multi_object_tracker/utils/utils.hpp" + +#include +#include +#include + #include #include #include @@ -26,17 +34,11 @@ #else #include #endif - -#define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" +#define EIGEN_MPL2_ONLY #include #include -#include -#include -#include using Label = autoware_auto_perception_msgs::msg::ObjectClassification; @@ -51,52 +53,59 @@ NormalVehicleTracker::NormalVehicleTracker( { object_ = object; - // initialize params - float q_stddev_x = 1.0; // object coordinate [m/s] - 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_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] - float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // map coordinate [rad] - float r_stddev_vx = 1.0; // object coordinate [m/s] - float p0_stddev_x = 1.0; // object coordinate [m/s] - 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_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_slip = std::pow(q_stddev_slip, 2.0); + // Initialize parameters + // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty + float r_stddev_x = 0.5; // in vehicle coordinate [m] + float r_stddev_y = 0.4; // in vehicle coordinate [m] + float r_stddev_yaw = tier4_autoware_utils::deg2rad(20); // in map coordinate [rad] + float r_stddev_vel = 1.0; // in object coordinate [m/s] 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); - ekf_params_.r_cov_vx = std::pow(r_stddev_vx, 2.0); + ekf_params_.r_cov_vel = std::pow(r_stddev_vel, 2.0); + // initial state covariance + float p0_stddev_x = 1.0; // in object coordinate [m] + float p0_stddev_y = 0.3; // in object coordinate [m] + float p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] + float p0_stddev_vel = tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] + float p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0); 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_vel = std::pow(p0_stddev_vel, 2.0); ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); - max_vx_ = tier4_autoware_utils::kmph2mps(100); // [m/s] - max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad/s] + // process noise covariance + ekf_params_.q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration + ekf_params_.q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration + ekf_params_.q_stddev_yaw_rate_min = + tier4_autoware_utils::deg2rad(1.5); // [rad/s] uncertain yaw change rate + ekf_params_.q_stddev_yaw_rate_max = + tier4_autoware_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate + float q_stddev_slip_rate_min = + tier4_autoware_utils::deg2rad(0.3); // [rad/s] uncertain slip angle change rate + float q_stddev_slip_rate_max = + tier4_autoware_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate + ekf_params_.q_cov_slip_rate_min = std::pow(q_stddev_slip_rate_min, 2.0); + ekf_params_.q_cov_slip_rate_max = std::pow(q_stddev_slip_rate_max, 2.0); + ekf_params_.q_max_slip_angle = tier4_autoware_utils::deg2rad(30); // [rad] max slip angle + // limitations + max_vel_ = tier4_autoware_utils::kmph2mps(100); // [m/s] + max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad] velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] - // initialize X matrix + // initialize state vector X Eigen::MatrixXd X(ekf_params_.dim_x, 1); 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::VEL) = object.kinematics.twist_with_covariance.twist.linear.x; } else { - X(IDX::VX) = 0.0; + X(IDX::VEL) = 0.0; } - X(IDX::SLIP) = 0.0; - // initialize P matrix + // initialize state covariance matrix P Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); if (!object.kinematics.has_position_covariance) { const double cos_yaw = std::cos(X(IDX::YAW)); @@ -111,7 +120,7 @@ NormalVehicleTracker::NormalVehicleTracker( ekf_params_.p0_cov_x * sin_yaw * sin_yaw + ekf_params_.p0_cov_y * cos_yaw * cos_yaw; 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::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; 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]; @@ -121,10 +130,10 @@ NormalVehicleTracker::NormalVehicleTracker( P(IDX::YAW, IDX::YAW) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; if (object.kinematics.has_twist_covariance) { - P(IDX::VX, IDX::VX) = + P(IDX::VEL, IDX::VEL) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } else { - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; + P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; } P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } @@ -132,8 +141,7 @@ NormalVehicleTracker::NormalVehicleTracker( if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; - last_input_bounding_box_ = { - object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; + last_input_bounding_box_ = bounding_box_; } else { // past default value // bounding_box_ = {4.0, 1.7, 2.0}; @@ -142,9 +150,7 @@ NormalVehicleTracker::NormalVehicleTracker( bounding_box_ = { bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; - last_input_bounding_box_ = { - bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, - bbox_object.shape.dimensions.z}; + last_input_bounding_box_ = bounding_box_; } ekf_.init(X, P); @@ -152,15 +158,28 @@ NormalVehicleTracker::NormalVehicleTracker( setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step // Set lf, lr - double point_ratio = 0.2; // under steered if smaller than 0.5 - lf_ = bounding_box_.length * point_ratio; - lr_ = bounding_box_.length * (1.0 - point_ratio); + lf_ = bounding_box_.length * 0.3; // 30% front from the center + lr_ = bounding_box_.length * 0.25; // 25% rear from the center } bool NormalVehicleTracker::predict(const rclcpp::Time & time) { const double dt = (time - last_update_time_).seconds(); - bool ret = predict(dt, ekf_); + if (dt < 0.0) { + RCLCPP_WARN(logger_, "dt is negative. (%f)", dt); + return false; + } + // if dt is too large, shorten dt and repeat prediction + const double dt_max = 0.11; + const uint32_t repeat = std::ceil(dt / dt_max); + const double dt_ = dt / repeat; + bool ret = false; + for (uint32_t i = 0; i < repeat; ++i) { + ret = predict(dt_, ekf_); + if (!ret) { + return false; + } + } if (ret) { last_update_time_ = time; } @@ -169,69 +188,116 @@ bool NormalVehicleTracker::predict(const rclcpp::Time & time) bool NormalVehicleTracker::predict(const double dt, KalmanFilter & ekf) const { - /* == Nonlinear model == static bicycle model + /* static bicycle model (constant slip angle, constant velocity) * - * 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 + * w_k = vel_k * sin(slip_k) / l_r + * x_{k+1} = x_k + vel_k*cos(yaw_k+slip_k)*dt - 0.5*w_k*vel_k*sin(yaw_k+slip_k)*dt*dt + * y_{k+1} = y_k + vel_k*sin(yaw_k+slip_k)*dt + 0.5*w_k*vel_k*cos(yaw_k+slip_k)*dt*dt + * yaw_{k+1} = yaw_k + w_k * dt + * vel_{k+1} = vel_k * slip_{k+1} = slip_k * */ - /* == Linearized model == + /* Jacobian Matrix + * + * A = [ 1, 0, -vel*sin(yaw+slip)*dt - 0.5*w_k*vel_k*cos(yaw+slip)*dt*dt, + * cos(yaw+slip)*dt - w*sin(yaw+slip)*dt*dt, + * -vel*sin(yaw+slip)*dt - 0.5*vel*vel/l_r*(cos(slip)sin(yaw+slip)+sin(slip)cos(yaw+slip))*dt*dt ] + * [ 0, 1, vel*cos(yaw+slip)*dt - 0.5*w_k*vel_k*sin(yaw+slip)*dt*dt, + * sin(yaw+slip)*dt + w*cos(yaw+slip)*dt*dt, + * vel*cos(yaw+slip)*dt + 0.5*vel*vel/l_r*(cos(slip)cos(yaw+slip)-sin(slip)sin(yaw+slip))*dt*dt ] + * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vel/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, -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 + // Current state vector 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) + X_t(IDX::SLIP)); const double sin_yaw = std::sin(X_t(IDX::YAW) + X_t(IDX::SLIP)); + const double vel = X_t(IDX::VEL); 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) + 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::SLIP) = X_t(IDX::SLIP); - // A + double w = vel * sin_slip / lr_; + const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); + const double w_dtdt = w * dt * dt; + const double vv_dtdt__lr = vel * vel * dt * dt / lr_; + + // Predict state vector X t+1 + Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state + X_next_t(IDX::X) = + X_t(IDX::X) + vel * cos_yaw * dt - 0.5 * vel * sin_slip * w_dtdt; // dx = v * cos(yaw) * dt + X_next_t(IDX::Y) = + X_t(IDX::Y) + vel * sin_yaw * dt + 0.5 * vel * cos_slip * w_dtdt; // dy = v * sin(yaw) * dt + X_next_t(IDX::YAW) = X_t(IDX::YAW) + w * dt; // d(yaw) = w * dt + X_next_t(IDX::VEL) = X_t(IDX::VEL); + X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // slip_angle = asin(lr * w / v) + + // State transition matrix A Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); - A(IDX::X, IDX::YAW) = -vx * sin_yaw * dt; - A(IDX::X, 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::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 + A(IDX::X, IDX::YAW) = -vel * sin_yaw * dt - 0.5 * vel * cos_yaw * w_dtdt; + A(IDX::X, IDX::VEL) = cos_yaw * dt - sin_yaw * w_dtdt; + A(IDX::X, IDX::SLIP) = + -vel * sin_yaw * dt - 0.5 * (cos_slip * sin_yaw + sin_slip * cos_yaw) * vv_dtdt__lr; + A(IDX::Y, IDX::YAW) = vel * cos_yaw * dt - 0.5 * vel * sin_yaw * w_dtdt; + A(IDX::Y, IDX::VEL) = sin_yaw * dt + cos_yaw * w_dtdt; + A(IDX::Y, IDX::SLIP) = + vel * cos_yaw * dt + 0.5 * (cos_slip * cos_yaw - sin_slip * sin_yaw) * vv_dtdt__lr; + A(IDX::YAW, IDX::VEL) = 1.0 / lr_ * sin_slip * dt; + A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt; + + // Process noise covariance Q + float q_stddev_yaw_rate{0.0}; + if (vel <= 0.01) { + q_stddev_yaw_rate = ekf_params_.q_stddev_yaw_rate_min; + } else { + // uncertainty of the yaw rate is limited by + // centripetal acceleration a_lat : d(yaw)/dt = w = a_lat/v + // or maximum slip angle slip_max : w = v*sin(slip_max)/l_r + q_stddev_yaw_rate = std::min( + ekf_params_.q_stddev_acc_lat / vel, + vel * std::sin(ekf_params_.q_max_slip_angle) / lr_); // [rad/s] + q_stddev_yaw_rate = std::min(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_max); + q_stddev_yaw_rate = std::max(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_min); + } + float q_cov_slip_rate{0.0f}; + if (vel <= 0.01) { + q_cov_slip_rate = ekf_params_.q_cov_slip_rate_min; + } else { + // The slip angle rate uncertainty is modeled as follows: + // d(slip)/dt ~ - sin(slip)/v * d(v)/dt + l_r/v * d(w)/dt + // where sin(slip) = w * l_r / v + // d(w)/dt is assumed to be proportional to w (more uncertain when slip is large) + // d(v)/dt and d(w)/t are considered to be uncorrelated + q_cov_slip_rate = + std::pow(ekf_params_.q_stddev_acc_lat * sin_slip / vel, 2) + std::pow(sin_slip * 1.5, 2); + q_cov_slip_rate = std::min(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_max); + q_cov_slip_rate = std::max(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_min); + } + const float q_cov_x = std::pow(0.5 * ekf_params_.q_stddev_acc_long * dt * dt, 2); + const float q_cov_y = std::pow(0.5 * ekf_params_.q_stddev_acc_lat * dt * dt, 2); + const float q_cov_yaw = std::pow(q_stddev_yaw_rate * dt, 2); + const float q_cov_vel = std::pow(ekf_params_.q_stddev_acc_long * dt, 2); + const float q_cov_slip = q_cov_slip_rate * dt * dt; + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); // Rotate the covariance matrix according to the vehicle yaw // because q_cov_x and y are in the vehicle coordinate system. - Q(IDX::X, IDX::X) = - (ekf_params_.q_cov_x * cos_yaw * cos_yaw + ekf_params_.q_cov_y * sin_yaw * sin_yaw) * dt * dt; - Q(IDX::X, IDX::Y) = (0.5f * (ekf_params_.q_cov_x - ekf_params_.q_cov_y) * sin_2yaw) * dt * dt; - Q(IDX::Y, IDX::Y) = - (ekf_params_.q_cov_x * sin_yaw * sin_yaw + ekf_params_.q_cov_y * cos_yaw * cos_yaw) * dt * dt; + Q(IDX::X, IDX::X) = (q_cov_x * cos_yaw * cos_yaw + q_cov_y * sin_yaw * sin_yaw); + Q(IDX::X, IDX::Y) = (0.5f * (q_cov_x - q_cov_y) * sin_2yaw); + Q(IDX::Y, IDX::Y) = (q_cov_x * sin_yaw * sin_yaw + q_cov_y * cos_yaw * cos_yaw); 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::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); + Q(IDX::YAW, IDX::YAW) = q_cov_yaw; + Q(IDX::VEL, IDX::VEL) = q_cov_vel; + Q(IDX::SLIP, IDX::SLIP) = q_cov_slip; + + // control-input model B and control-input u are not used + // 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); if (!ekf.predict(X_next_t, A, Q)) { RCLCPP_WARN(logger_, "Cannot predict"); @@ -253,8 +319,8 @@ bool NormalVehicleTracker::measureWithPose( r_cov_x = ekf_params_.r_cov_x; r_cov_y = ekf_params_.r_cov_y; } else if (utils::isLargeVehicleLabel(label)) { - constexpr float r_stddev_x = 8.0; // [m] - constexpr float r_stddev_y = 0.8; // [m] + constexpr float r_stddev_x = 2.0; // [m] + constexpr float r_stddev_y = 2.0; // [m] r_cov_x = std::pow(r_stddev_x, 2.0); r_cov_y = std::pow(r_stddev_y, 2.0); } else { @@ -262,35 +328,25 @@ bool NormalVehicleTracker::measureWithPose( r_cov_y = ekf_params_.r_cov_y; } - // extract current state - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - ekf_.getX(X_t); - // Decide dimension of measurement vector bool enable_velocity_measurement = false; if (object.kinematics.has_twist) { - const double predicted_vx = X_t(IDX::VX); - const double observed_vx = object.kinematics.twist_with_covariance.twist.linear.x; + Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state + ekf_.getX(X_t); + const double predicted_vel = X_t(IDX::VEL); + const double observed_vel = object.kinematics.twist_with_covariance.twist.linear.x; - if (std::fabs(predicted_vx - observed_vx) < velocity_deviation_threshold_) { + if (std::fabs(predicted_vel - observed_vel) < velocity_deviation_threshold_) { // Velocity deviation is small enable_velocity_measurement = true; } } - // pos x, pos y, yaw, vx depending on pose output + // pos x, pos y, yaw, vel depending on pose measurement const int dim_y = enable_velocity_measurement ? 4 : 3; - double measurement_yaw = tier4_autoware_utils::normalizeRadian( - tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); - { - // Fixed measurement_yaw to be in the range of +-90 degrees of X_t(IDX::YAW) - while (M_PI_2 <= X_t(IDX::YAW) - measurement_yaw) { - measurement_yaw = measurement_yaw + M_PI; - } - while (M_PI_2 <= measurement_yaw - X_t(IDX::YAW)) { - measurement_yaw = measurement_yaw - M_PI; - } - } + double measurement_yaw = getMeasurementYaw(object); // get sign-solved yaw angle + Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state + ekf_.getX(X_t); // convert to boundingbox if input is convex shape autoware_auto_perception_msgs::msg::DetectedObject bbox_object; @@ -342,30 +398,30 @@ bool NormalVehicleTracker::measureWithPose( // Update the velocity when necessary if (dim_y == 4) { - Y(IDX::VX, 0) = object.kinematics.twist_with_covariance.twist.linear.x; - C(3, IDX::VX) = 1.0; // for vx + Y(IDX::VEL, 0) = object.kinematics.twist_with_covariance.twist.linear.x; + C(3, IDX::VEL) = 1.0; // for vel if (!object.kinematics.has_twist_covariance) { - R(3, 3) = ekf_params_.r_cov_vx; // vx -vx + R(3, 3) = ekf_params_.r_cov_vel; // vel -vel } else { R(3, 3) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } } - // ekf update: this tracks tracking point + // ekf update if (!ekf_.update(Y, C, R)) { RCLCPP_WARN(logger_, "Cannot update"); } - // normalize yaw and limit vx, wz + // normalize yaw and limit vel, slip { Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); ekf_.getX(X_t); ekf_.getP(P_t); X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); - 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_vel_ <= X_t(IDX::VEL) && X_t(IDX::VEL) <= max_vel_)) { + X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -max_vel_ : max_vel_; } 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_; @@ -383,9 +439,8 @@ bool NormalVehicleTracker::measureWithPose( bool NormalVehicleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { + // if the input shape is convex type, convert it to bbox type autoware_auto_perception_msgs::msg::DetectedObject bbox_object; - - // if input is convex shape convert it to bbox shape if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { utils::convertConvexHullToBoundingBox(object, bbox_object); } else { @@ -400,6 +455,11 @@ bool NormalVehicleTracker::measureWithShape( gain * bounding_box_.height + (1.0 - gain) * bbox_object.shape.dimensions.z; last_input_bounding_box_ = { bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; + + // update lf, lr + lf_ = bounding_box_.length * 0.3; // 30% front from the center + lr_ = bounding_box_.length * 0.25; // 25% rear from the center + return true; } @@ -422,12 +482,6 @@ bool NormalVehicleTracker::measure( measureWithPose(object); measureWithShape(object); - // refinement - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); - ekf_.getX(X_t); - ekf_.getP(P_t); - /* calc nearest corner index*/ setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step @@ -441,7 +495,7 @@ bool NormalVehicleTracker::getTrackedObject( object.object_id = getUUID(); object.classification = getClassification(); - // predict kinematics + // predict state KalmanFilter tmp_ekf_for_no_update = ekf_; const double dt = (time - last_update_time_).seconds(); if (0.001 /*1msec*/ < dt) { @@ -452,6 +506,7 @@ bool NormalVehicleTracker::getTrackedObject( tmp_ekf_for_no_update.getX(X_t); tmp_ekf_for_no_update.getP(P); + /* 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; @@ -496,23 +551,44 @@ bool NormalVehicleTracker::getTrackedObject( pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); // twist - twist_with_cov.twist.linear.x = X_t(IDX::VX) * std::cos(X_t(IDX::SLIP)); - twist_with_cov.twist.linear.y = X_t(IDX::VX) * std::sin(X_t(IDX::SLIP)); + twist_with_cov.twist.linear.x = X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)); + twist_with_cov.twist.linear.y = X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)); 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 + X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)) / lr_; // yaw_rate = vel_k * sin(slip_k) / l_r + /* twist covariance + * convert covariance from velocity, slip angle to vx, vy, and yaw angle + * + * vx = vel * cos(slip) + * vy = vel * sin(slip) + * wz = vel * sin(slip) / l_r = vy / l_r + * + */ + constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - 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; + + Eigen::MatrixXd cov_jacob(3, 2); + cov_jacob << std::cos(X_t(IDX::SLIP)), -X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)), + std::sin(X_t(IDX::SLIP)), X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)), + std::sin(X_t(IDX::SLIP)) / lr_, X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)) / lr_; + Eigen::MatrixXd cov_twist(2, 2); + cov_twist << P(IDX::VEL, IDX::VEL), P(IDX::VEL, IDX::SLIP), P(IDX::SLIP, IDX::VEL), + P(IDX::SLIP, IDX::SLIP); + Eigen::MatrixXd twist_cov_mat = cov_jacob * cov_twist * cov_jacob.transpose(); + + twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = twist_cov_mat(0, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = twist_cov_mat(0, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = twist_cov_mat(0, 2); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = twist_cov_mat(1, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = twist_cov_mat(1, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_YAW] = twist_cov_mat(1, 2); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = twist_cov_mat(2, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_Y] = twist_cov_mat(2, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = twist_cov_mat(2, 2); 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::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::SLIP, IDX::SLIP); // set shape object.shape.dimensions.x = bounding_box_.length; @@ -534,3 +610,22 @@ void NormalVehicleTracker::setNearestCornerOrSurfaceIndex( X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length, self_transform); } + +double NormalVehicleTracker::getMeasurementYaw( + const autoware_auto_perception_msgs::msg::DetectedObject & object) +{ + double measurement_yaw = tier4_autoware_utils::normalizeRadian( + tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); + { + Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); + ekf_.getX(X_t); + // Fixed measurement_yaw to be in the range of +-90 degrees of X_t(IDX::YAW) + while (M_PI_2 <= X_t(IDX::YAW) - measurement_yaw) { + measurement_yaw = measurement_yaw + M_PI; + } + while (M_PI_2 <= measurement_yaw - X_t(IDX::YAW)) { + measurement_yaw = measurement_yaw - M_PI; + } + } + return measurement_yaw; +} From 0f7f20e2668a833de0e31b574c229b8c6a74b0a2 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 7 Feb 2024 00:20:45 +0900 Subject: [PATCH 2/4] fix(multi_object_tracker): bicycle motion model - set minimum wheel-to-center length (#6337) * fix: bicycle motion model - set minimum wheel-to-center length for stability Signed-off-by: Taekjin LEE * chore: align comments Signed-off-by: Taekjin LEE --------- Signed-off-by: Taekjin LEE --- .../src/tracker/model/bicycle_tracker.cpp | 8 ++++---- .../src/tracker/model/big_vehicle_tracker.cpp | 9 ++++----- .../src/tracker/model/normal_vehicle_tracker.cpp | 8 ++++---- 3 files changed, 12 insertions(+), 13 deletions(-) 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 0b56b11451ad3..8133bc7dcf0a0 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -143,8 +143,8 @@ BicycleTracker::BicycleTracker( ekf_.init(X, P); // Set lf, lr - lf_ = bounding_box_.length * 0.3; // 30% front from the center - lr_ = bounding_box_.length * 0.3; // 30% rear from the center + lf_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% front from the center, minimum of 0.3m + lr_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% rear from the center, minimum of 0.3m } bool BicycleTracker::predict(const rclcpp::Time & time) @@ -425,8 +425,8 @@ bool BicycleTracker::measureWithShape( bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; // update lf, lr - lf_ = bounding_box_.length * 0.3; // 30% front from the center - lr_ = bounding_box_.length * 0.3; // 30% rear from the center + lf_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% front from the center, minimum of 0.3m + lr_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% rear from the center, minimum of 0.3m return true; } 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 34a72437d7721..b5a730e93663a 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 @@ -158,8 +158,8 @@ BigVehicleTracker::BigVehicleTracker( setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step // Set lf, lr - lf_ = bounding_box_.length * 0.3; // 30% front from the center - lr_ = bounding_box_.length * 0.25; // 25% rear from the center + lf_ = std::max(bounding_box_.length * 0.3, 1.5); // 30% front from the center, minimum of 1.5m + lr_ = std::max(bounding_box_.length * 0.25, 1.5); // 25% rear from the center, minimum of 1.5m } bool BigVehicleTracker::predict(const rclcpp::Time & time) @@ -457,9 +457,8 @@ bool BigVehicleTracker::measureWithShape( bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; // update lf, lr - lf_ = bounding_box_.length * 0.3; // 30% front from the center - lr_ = bounding_box_.length * 0.25; // 25% rear from the center - + lf_ = std::max(bounding_box_.length * 0.3, 1.5); // 30% front from the center, minimum of 1.5m + lr_ = std::max(bounding_box_.length * 0.25, 1.5); // 25% rear from the center, minimum of 1.5m return true; } 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 fcf72360b84c2..ca0a2d39c266b 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 @@ -158,8 +158,8 @@ NormalVehicleTracker::NormalVehicleTracker( setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step // Set lf, lr - lf_ = bounding_box_.length * 0.3; // 30% front from the center - lr_ = bounding_box_.length * 0.25; // 25% rear from the center + lf_ = std::max(bounding_box_.length * 0.3, 1.0); // 30% front from the center, minimum of 1.0m + lr_ = std::max(bounding_box_.length * 0.25, 1.0); // 25% rear from the center, minimum of 1.0m } bool NormalVehicleTracker::predict(const rclcpp::Time & time) @@ -457,8 +457,8 @@ bool NormalVehicleTracker::measureWithShape( bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; // update lf, lr - lf_ = bounding_box_.length * 0.3; // 30% front from the center - lr_ = bounding_box_.length * 0.25; // 25% rear from the center + lf_ = std::max(bounding_box_.length * 0.3, 1.0); // 30% front from the center, minimum of 1.0m + lr_ = std::max(bounding_box_.length * 0.25, 1.0); // 25% rear from the center, minimum of 1.0m return true; } From db82695aa4531b67f607f99eeda9a10ed701f840 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 8 Feb 2024 15:17:55 +0900 Subject: [PATCH 3/4] fix(multi_object_tracker): tune parameters for pedestrian tracker (#6343) * fix: tune parameters for pedestrian initial covariances and process noise Signed-off-by: Taekjin LEE * chore: refactoring comments, aligning trackers Signed-off-by: Taekjin LEE --------- Signed-off-by: Taekjin LEE --- .../src/tracker/model/bicycle_tracker.cpp | 18 ++--- .../src/tracker/model/big_vehicle_tracker.cpp | 11 +-- .../tracker/model/normal_vehicle_tracker.cpp | 10 +-- .../src/tracker/model/pedestrian_tracker.cpp | 74 ++++++++++--------- 4 files changed, 56 insertions(+), 57 deletions(-) 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 8133bc7dcf0a0..8a3a5629ec277 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -173,7 +173,7 @@ bool BicycleTracker::predict(const rclcpp::Time & time) bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const { - /* static bicycle model (constant slip angle, constant velocity) + /* Motion model: static bicycle model (constant slip angle, constant velocity) * * w_k = vel_k * sin(slip_k) / l_r * x_{k+1} = x_k + vel_k*cos(yaw_k+slip_k)*dt - 0.5*w_k*vel_k*sin(yaw_k+slip_k)*dt*dt @@ -291,12 +291,6 @@ bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const return true; } -/** - * @brief measurement update with ekf on receiving detected_object - * - * @param object : Detected object - * @return bool : measurement is updatable - */ bool BicycleTracker::measureWithPose( const autoware_auto_perception_msgs::msg::DetectedObject & object) { @@ -333,19 +327,16 @@ bool BicycleTracker::measureWithPose( const int dim_y = use_orientation_information ? 3 : 2; // pos x, pos y, (pos yaw) depending on Pose output - /* Set measurement matrix */ + // Set measurement matrix C and observation vector Y Eigen::MatrixXd Y(dim_y, 1); + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); Y(IDX::X, 0) = object.kinematics.pose_with_covariance.pose.position.x; Y(IDX::Y, 0) = object.kinematics.pose_with_covariance.pose.position.y; - - /* Set measurement matrix */ - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); C(0, IDX::X) = 1.0; // for pos x C(1, IDX::Y) = 1.0; // for pos y - /* Set measurement noise covariance */ + // Set noise covariance matrix R Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - if (!object.kinematics.has_position_covariance) { R(0, 0) = ekf_params_.r_cov_x; // x - x R(0, 1) = 0.0; // x - y @@ -471,6 +462,7 @@ bool BicycleTracker::getTrackedObject( tmp_ekf_for_no_update.getX(X_t); tmp_ekf_for_no_update.getP(P); + /* 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; 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 b5a730e93663a..80c0df7e5ffb1 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 @@ -188,7 +188,7 @@ bool BigVehicleTracker::predict(const rclcpp::Time & time) bool BigVehicleTracker::predict(const double dt, KalmanFilter & ekf) const { - /* static bicycle model (constant slip angle, constant velocity) + /* Motion model: static bicycle model (constant slip angle, constant velocity) * * w_k = vel_k * sin(slip_k) / l_r * x_{k+1} = x_k + vel_k*cos(yaw_k+slip_k)*dt - 0.5*w_k*vel_k*sin(yaw_k+slip_k)*dt*dt @@ -362,11 +362,9 @@ bool BigVehicleTracker::measureWithPose( last_input_bounding_box_.width, last_input_bounding_box_.length, last_nearest_corner_index_, bbox_object, X_t(IDX::YAW), offset_object, tracking_offset_); - /* Set measurement matrix and noise covariance*/ + // Set measurement matrix C and observation vector Y Eigen::MatrixXd Y(dim_y, 1); Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); - Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - Y(IDX::X, 0) = offset_object.kinematics.pose_with_covariance.pose.position.x; Y(IDX::Y, 0) = offset_object.kinematics.pose_with_covariance.pose.position.y; Y(IDX::YAW, 0) = measurement_yaw; @@ -374,7 +372,8 @@ bool BigVehicleTracker::measureWithPose( C(1, IDX::Y) = 1.0; // for pos y C(2, IDX::YAW) = 1.0; // for yaw - /* Set measurement noise covariance */ + // Set noise covariance matrix R + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); if (!object.kinematics.has_position_covariance) { const double cos_yaw = std::cos(measurement_yaw); const double sin_yaw = std::sin(measurement_yaw); @@ -459,6 +458,7 @@ bool BigVehicleTracker::measureWithShape( // update lf, lr lf_ = std::max(bounding_box_.length * 0.3, 1.5); // 30% front from the center, minimum of 1.5m lr_ = std::max(bounding_box_.length * 0.25, 1.5); // 25% rear from the center, minimum of 1.5m + return true; } @@ -597,6 +597,7 @@ bool BigVehicleTracker::getTrackedObject( const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); object.shape.footprint = tier4_autoware_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); + return true; } 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 ca0a2d39c266b..7714c381894f0 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 @@ -188,7 +188,7 @@ bool NormalVehicleTracker::predict(const rclcpp::Time & time) bool NormalVehicleTracker::predict(const double dt, KalmanFilter & ekf) const { - /* static bicycle model (constant slip angle, constant velocity) + /* Motion model: static bicycle model (constant slip angle, constant velocity) * * w_k = vel_k * sin(slip_k) / l_r * x_{k+1} = x_k + vel_k*cos(yaw_k+slip_k)*dt - 0.5*w_k*vel_k*sin(yaw_k+slip_k)*dt*dt @@ -362,11 +362,9 @@ bool NormalVehicleTracker::measureWithPose( last_input_bounding_box_.width, last_input_bounding_box_.length, last_nearest_corner_index_, bbox_object, X_t(IDX::YAW), offset_object, tracking_offset_); - /* Set measurement matrix and noise covariance*/ + // Set measurement matrix C and observation vector Y Eigen::MatrixXd Y(dim_y, 1); Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); - Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - Y(IDX::X, 0) = offset_object.kinematics.pose_with_covariance.pose.position.x; Y(IDX::Y, 0) = offset_object.kinematics.pose_with_covariance.pose.position.y; Y(IDX::YAW, 0) = measurement_yaw; @@ -374,7 +372,8 @@ bool NormalVehicleTracker::measureWithPose( C(1, IDX::Y) = 1.0; // for pos y C(2, IDX::YAW) = 1.0; // for yaw - /* Set measurement noise covariance */ + // Set noise covariance matrix R + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); if (!object.kinematics.has_position_covariance) { const double cos_yaw = std::cos(measurement_yaw); const double sin_yaw = std::sin(measurement_yaw); @@ -598,6 +597,7 @@ bool NormalVehicleTracker::getTrackedObject( const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); object.shape.footprint = tier4_autoware_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); + return true; } 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..57c2e8f899951 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -19,7 +19,6 @@ #include "multi_object_tracker/tracker/model/pedestrian_tracker.hpp" #include "multi_object_tracker/utils/utils.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -35,6 +34,7 @@ #else #include #endif +#include "object_recognition_utils/object_recognition_utils.hpp" #define EIGEN_MPL2_ONLY #include @@ -52,37 +52,41 @@ PedestrianTracker::PedestrianTracker( { object_ = object; - // initialize params - float q_stddev_x = 0.4; // [m/s] - 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_wz = tier4_autoware_utils::deg2rad(20); // [rad/(s*s)] - float r_stddev_x = 0.4; // [m] - float r_stddev_y = 0.4; // [m] - float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] - float p0_stddev_x = 1.0; // [m/s] - 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_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_wz = std::pow(q_stddev_wz, 2.0); + // Initialize parameters + // measurement noise covariance + float r_stddev_x = 0.4; // [m] + float r_stddev_y = 0.4; // [m] + float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] 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); + // initial state covariance + float p0_stddev_x = 2.0; // [m] + float p0_stddev_y = 2.0; // [m] + float p0_stddev_yaw = tier4_autoware_utils::deg2rad(1000); // [rad] + float p0_stddev_vx = tier4_autoware_utils::kmph2mps(120); // [m/s] + float p0_stddev_wz = tier4_autoware_utils::deg2rad(360); // [rad/s] ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0); 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); + // process noise covariance + float q_stddev_x = 0.4; // [m/s] + 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_wz = tier4_autoware_utils::deg2rad(30); // [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); + // limitations max_vx_ = tier4_autoware_utils::kmph2mps(10); // [m/s] max_wz_ = tier4_autoware_utils::deg2rad(30); // [rad/s] - // initialize X matrix + // initialize state vector X Eigen::MatrixXd X(ekf_params_.dim_x, 1); X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y; @@ -95,7 +99,7 @@ PedestrianTracker::PedestrianTracker( X(IDX::WZ) = 0.0; } - // initialize P matrix + // initialize state covariance matrix P Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); if (!object.kinematics.has_position_covariance) { const double cos_yaw = std::cos(X(IDX::YAW)); @@ -154,7 +158,7 @@ bool PedestrianTracker::predict(const rclcpp::Time & time) bool PedestrianTracker::predict(const double dt, KalmanFilter & ekf) const { - /* == Nonlinear model == + /* Motion model: Constant turn-rate motion model (CTRV) * * x_{k+1} = x_k + vx_k * cos(yaw_k) * dt * y_{k+1} = y_k + vx_k * sin(yaw_k) * dt @@ -164,7 +168,7 @@ bool PedestrianTracker::predict(const double dt, KalmanFilter & ekf) const * */ - /* == Linearized model == + /* Jacobian Matrix * * A = [ 1, 0, -vx*sin(yaw)*dt, cos(yaw)*dt, 0] * [ 0, 1, vx*cos(yaw)*dt, sin(yaw)*dt, 0] @@ -173,14 +177,14 @@ bool PedestrianTracker::predict(const double dt, KalmanFilter & ekf) const * [ 0, 0, 0, 0, 1] */ - // X t + // Current state vector 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 sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); - // X t+1 + // Predict state vector 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) @@ -188,7 +192,7 @@ bool PedestrianTracker::predict(const double dt, KalmanFilter & ekf) const X_next_t(IDX::VX) = X_t(IDX::VX); X_next_t(IDX::WZ) = X_t(IDX::WZ); - // A + // State transition matrix 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::VX) = cos_yaw * dt; @@ -196,7 +200,7 @@ bool PedestrianTracker::predict(const double dt, KalmanFilter & ekf) const A(IDX::Y, IDX::VX) = sin_yaw * dt; A(IDX::YAW, IDX::WZ) = dt; - // Q + // Process noise covariance Q Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); // Rotate the covariance matrix according to the vehicle yaw // because q_cov_x and y are in the vehicle coordinate system. @@ -240,18 +244,16 @@ bool PedestrianTracker::measureWithPose( // if (tier4_autoware_utils::deg2rad(60) < std::fabs(theta)) return false; // } - /* Set measurement matrix */ + // Set measurement matrix C and observation vector Y Eigen::MatrixXd Y(dim_y, 1); + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); Y << object.kinematics.pose_with_covariance.pose.position.x, object.kinematics.pose_with_covariance.pose.position.y; - - /* Set measurement matrix */ - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); C(0, IDX::X) = 1.0; // for pos x C(1, IDX::Y) = 1.0; // for pos y // C(2, IDX::YAW) = 1.0; // for yaw - /* Set measurement noise covariance */ + // Set noise covariance matrix R Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); if (!object.kinematics.has_position_covariance) { R(0, 0) = ekf_params_.r_cov_x; // x - x @@ -270,6 +272,8 @@ bool PedestrianTracker::measureWithPose( // R(2, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_Y]; // R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; } + + // ekf update if (!ekf_.update(Y, C, R)) { RCLCPP_WARN(logger_, "Cannot update"); } @@ -345,7 +349,7 @@ bool PedestrianTracker::getTrackedObject( object.object_id = getUUID(); object.classification = getClassification(); - // predict kinematics + // predict state KalmanFilter tmp_ekf_for_no_update = ekf_; const double dt = (time - last_update_time_).seconds(); if (0.001 /*1msec*/ < dt) { @@ -356,6 +360,7 @@ bool PedestrianTracker::getTrackedObject( tmp_ekf_for_no_update.getX(X_t); tmp_ekf_for_no_update.getP(P); + /* 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; @@ -399,6 +404,7 @@ bool PedestrianTracker::getTrackedObject( constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + 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; From f098d3a8d40ecb02ae2772672696dad2604f90f9 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 21 Feb 2024 10:59:03 +0900 Subject: [PATCH 4/4] fix(tracking_object_merger): merge velocity base on target object coordinate system (#6457) fix(tracking_object_merger): merge velocity base on target object coordinate Signed-off-by: Taekjin LEE --- .../src/utils/utils.cpp | 68 ++++++++++++------- 1 file changed, 45 insertions(+), 23 deletions(-) diff --git a/perception/tracking_object_merger/src/utils/utils.cpp b/perception/tracking_object_merger/src/utils/utils.cpp index 85ab105e61038..4901c2fe09f9c 100644 --- a/perception/tracking_object_merger/src/utils/utils.cpp +++ b/perception/tracking_object_merger/src/utils/utils.cpp @@ -324,43 +324,61 @@ autoware_auto_perception_msgs::msg::TrackedObjectKinematics objectKinematicsVXMe // copy main object at first output_kinematics = main_obj.kinematics; auto sub_obj_ = sub_obj; - // do not merge reverse direction objects + // do not merge if motion direction is different if (!objectsHaveSameMotionDirections(main_obj, sub_obj)) { return output_kinematics; - } else if (objectsYawIsReverted(main_obj, sub_obj)) { - // revert velocity (revert pose is not necessary because it is not used in tracking) - sub_obj_.kinematics.twist_with_covariance.twist.linear.x *= -1.0; - sub_obj_.kinematics.twist_with_covariance.twist.linear.y *= -1.0; } // currently only merge vx if (policy == MergePolicy::SKIP) { return output_kinematics; } else if (policy == MergePolicy::OVERWRITE) { - output_kinematics.twist_with_covariance.twist.linear.x = - sub_obj_.kinematics.twist_with_covariance.twist.linear.x; + // use main_obj's orientation + // take sub_obj's velocity vector and convert into main_obj's frame, but take only x component + const auto sub_vx = sub_obj_.kinematics.twist_with_covariance.twist.linear.x; + const auto sub_vy = sub_obj_.kinematics.twist_with_covariance.twist.linear.y; + const auto main_yaw = tf2::getYaw(main_obj.kinematics.pose_with_covariance.pose.orientation); + const auto sub_yaw = tf2::getYaw(sub_obj_.kinematics.pose_with_covariance.pose.orientation); + const auto sub_vx_in_main_frame = + sub_vx * std::cos(sub_yaw - main_yaw) + sub_vy * std::sin(sub_yaw - main_yaw); + output_kinematics.twist_with_covariance.twist.linear.x = sub_vx_in_main_frame; + return output_kinematics; } else if (policy == MergePolicy::FUSION) { + // use main_obj's orientation + // take main_obj's velocity vector and convert into sub_obj's frame, but take only x component const auto main_vx = main_obj.kinematics.twist_with_covariance.twist.linear.x; const auto sub_vx = sub_obj_.kinematics.twist_with_covariance.twist.linear.x; + const auto sub_vy = sub_obj_.kinematics.twist_with_covariance.twist.linear.y; + const auto main_yaw = tf2::getYaw(main_obj.kinematics.pose_with_covariance.pose.orientation); + const auto sub_yaw = tf2::getYaw(sub_obj_.kinematics.pose_with_covariance.pose.orientation); + const auto sub_vel_in_main_frame_x = + sub_vx * std::cos(sub_yaw - main_yaw) + sub_vy * std::sin(sub_yaw - main_yaw); // inverse weight const auto main_vx_cov = main_obj.kinematics.twist_with_covariance.covariance[0]; const auto sub_vx_cov = sub_obj_.kinematics.twist_with_covariance.covariance[0]; + const auto sub_vy_cov = sub_obj_.kinematics.twist_with_covariance.covariance[7]; + const auto sub_vel_cov_in_main_frame_x = + sub_vx_cov * std::cos(sub_yaw - main_yaw) * std::cos(sub_yaw - main_yaw) + + sub_vy_cov * std::sin(sub_yaw - main_yaw) * std::sin(sub_yaw - main_yaw); double main_vx_weight, sub_vx_weight; if (main_vx_cov == 0.0) { main_vx_weight = 1.0 * 1e6; } else { main_vx_weight = 1.0 / main_vx_cov; } - if (sub_vx_cov == 0.0) { + if (sub_vel_cov_in_main_frame_x == 0.0) { sub_vx_weight = 1.0 * 1e6; } else { - sub_vx_weight = 1.0 / sub_vx_cov; + sub_vx_weight = 1.0 / sub_vel_cov_in_main_frame_x; } - // merge with covariance + + // merge velocity with covariance output_kinematics.twist_with_covariance.twist.linear.x = - (main_vx * main_vx_weight + sub_vx * sub_vx_weight) / (main_vx_weight + sub_vx_weight); + (main_vx * main_vx_weight + sub_vel_in_main_frame_x * sub_vx_weight) / + (main_vx_weight + sub_vx_weight); output_kinematics.twist_with_covariance.covariance[0] = 1.0 / (main_vx_weight + sub_vx_weight); + return output_kinematics; } else { std::cerr << "unknown merge policy in objectKinematicsMerger function." << std::endl; @@ -453,21 +471,25 @@ autoware_auto_perception_msgs::msg::Shape shapeMerger( void updateExceptVelocity(TrackedObject & main_obj, const TrackedObject & sub_obj) { + // do not update if motion direction is different if (!objectsHaveSameMotionDirections(main_obj, sub_obj)) { - // do not update velocity - return; - } else if (objectsYawIsReverted(main_obj, sub_obj)) { - // revert velocity (revert pose is not necessary because it is not used in tracking) - const auto vx_temp = sub_obj.kinematics.twist_with_covariance.twist.linear.x; - main_obj = sub_obj; - main_obj.kinematics.twist_with_covariance.twist.linear.x = -vx_temp; return; - } else { - // update velocity - const auto vx_temp = sub_obj.kinematics.twist_with_covariance.twist.linear.x; - main_obj = sub_obj; - main_obj.kinematics.twist_with_covariance.twist.linear.x = vx_temp; } + // take main_obj's velocity vector and convert into sub_obj's frame + // use sub_obj's orientation, but take only x component + const auto main_vx = main_obj.kinematics.twist_with_covariance.twist.linear.x; + const auto main_vy = main_obj.kinematics.twist_with_covariance.twist.linear.y; + const auto main_yaw = tf2::getYaw(main_obj.kinematics.pose_with_covariance.pose.orientation); + const auto sub_yaw = tf2::getYaw(sub_obj.kinematics.pose_with_covariance.pose.orientation); + const auto main_vx_in_sub_frame = + main_vx * std::cos(main_yaw - sub_yaw) + main_vy * std::sin(main_yaw - sub_yaw); + + // copy sub object to fused object + main_obj = sub_obj; + // recover main object's velocity + main_obj.kinematics.twist_with_covariance.twist.linear.x = main_vx_in_sub_frame; + + return; } void updateOnlyObjectVelocity(TrackedObject & main_obj, const TrackedObject & sub_obj)