From 7cfd2672d618cc444a689f0ffbf2d19437f79736 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 15 Jan 2024 18:23:16 +0900 Subject: [PATCH] chore: explanations of Kalman filter matrices Signed-off-by: Taekjin LEE --- .../src/data_association/data_association.cpp | 2 +- .../src/tracker/model/bicycle_tracker.cpp | 20 ++++++++++--------- .../src/tracker/model/big_vehicle_tracker.cpp | 20 ++++++++++--------- .../tracker/model/normal_vehicle_tracker.cpp | 20 ++++++++++--------- 4 files changed, 34 insertions(+), 28 deletions(-) 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 c4d308bf11232..a0502e0238c62 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 = true; + if (2.448 /*95%*/ <= mahalanobis_dist) passed_gate = true; // gate disabled } // 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 aeab6b5d83228..c116f81a65224 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -60,7 +60,7 @@ BicycleTracker::BicycleTracker( 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 covariance + // 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] @@ -87,7 +87,7 @@ BicycleTracker::BicycleTracker( max_vel_ = tier4_autoware_utils::kmph2mps(100); // [m/s] max_slip_ = 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; @@ -99,7 +99,7 @@ BicycleTracker::BicycleTracker( } 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) { @@ -197,7 +197,7 @@ bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const * */ - // 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)); @@ -210,7 +210,7 @@ bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const const double w_dtdt = w * dt * dt; const double vv_dtdt__lr = vel * vel * dt * dt / lr_; - // 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) + vel * cos_yaw * dt - 0.5 * vel * sin_slip * w_dtdt; // dx = v * cos(yaw) @@ -220,7 +220,7 @@ bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const X_next_t(IDX::VEL) = X_t(IDX::VEL); X_next_t(IDX::SLIP) = X_t(IDX::SLIP); - // A + // State transition matrix A Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); 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; @@ -233,7 +233,7 @@ bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const A(IDX::YAW, IDX::VEL) = 1.0 / lr_ * sin_slip * dt; A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt; - // Q + // 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; @@ -276,8 +276,10 @@ bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const Q(IDX::YAW, IDX::YAW) = q_cov_yaw; Q(IDX::VEL, IDX::VEL) = q_cov_vel; Q(IDX::SLIP, IDX::SLIP) = q_cov_slip; - 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); + + // 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"); 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 b61682004d496..211de68657448 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 @@ -61,7 +61,7 @@ BigVehicleTracker::BigVehicleTracker( 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_vel = std::pow(r_stddev_vel, 2.0); - // initial covariance + // 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] @@ -89,7 +89,7 @@ BigVehicleTracker::BigVehicleTracker( 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; @@ -102,7 +102,7 @@ BigVehicleTracker::BigVehicleTracker( 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)); @@ -212,7 +212,7 @@ bool BigVehicleTracker::predict(const double dt, KalmanFilter & ekf) const * */ - // 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)); @@ -225,7 +225,7 @@ bool BigVehicleTracker::predict(const double dt, KalmanFilter & ekf) const const double w_dtdt = w * dt * dt; const double vv_dtdt__lr = vel * vel * dt * dt / lr_; - // 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) + vel * cos_yaw * dt - 0.5 * vel * sin_slip * w_dtdt; // dx = v * cos(yaw) @@ -235,7 +235,7 @@ bool BigVehicleTracker::predict(const double dt, KalmanFilter & ekf) const X_next_t(IDX::VEL) = X_t(IDX::VEL); X_next_t(IDX::SLIP) = X_t(IDX::SLIP); - // A + // State transition matrix A Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); 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; @@ -248,7 +248,7 @@ bool BigVehicleTracker::predict(const double dt, KalmanFilter & ekf) const A(IDX::YAW, IDX::VEL) = 1.0 / lr_ * sin_slip * dt; A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt; - // Q + // 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; @@ -291,8 +291,10 @@ bool BigVehicleTracker::predict(const double dt, KalmanFilter & ekf) const Q(IDX::YAW, IDX::YAW) = q_cov_yaw; Q(IDX::VEL, IDX::VEL) = q_cov_vel; Q(IDX::SLIP, IDX::SLIP) = q_cov_slip; - 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); + + // 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"); 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 970a79fa47bac..1ee13b58a58dc 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 @@ -61,7 +61,7 @@ NormalVehicleTracker::NormalVehicleTracker( 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_vel = std::pow(r_stddev_vel, 2.0); - // initial covariance + // 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] @@ -89,7 +89,7 @@ NormalVehicleTracker::NormalVehicleTracker( max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad/s] 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; @@ -101,7 +101,7 @@ NormalVehicleTracker::NormalVehicleTracker( } 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)); @@ -212,7 +212,7 @@ bool NormalVehicleTracker::predict(const double dt, KalmanFilter & ekf) const * */ - // 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)); @@ -225,7 +225,7 @@ bool NormalVehicleTracker::predict(const double dt, KalmanFilter & ekf) const const double w_dtdt = w * dt * dt; const double vv_dtdt__lr = vel * vel * dt * dt / lr_; - // 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) + vel * cos_yaw * dt - 0.5 * vel * sin_slip * w_dtdt; // dx = v * cos(yaw) @@ -235,7 +235,7 @@ bool NormalVehicleTracker::predict(const double dt, KalmanFilter & ekf) const X_next_t(IDX::VEL) = X_t(IDX::VEL); X_next_t(IDX::SLIP) = X_t(IDX::SLIP); - // A + // State transition matrix A Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); 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; @@ -248,7 +248,7 @@ bool NormalVehicleTracker::predict(const double dt, KalmanFilter & ekf) const A(IDX::YAW, IDX::VEL) = 1.0 / lr_ * sin_slip * dt; A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt; - // Q + // 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; @@ -291,8 +291,10 @@ bool NormalVehicleTracker::predict(const double dt, KalmanFilter & ekf) const Q(IDX::YAW, IDX::YAW) = q_cov_yaw; Q(IDX::VEL, IDX::VEL) = q_cov_vel; Q(IDX::SLIP, IDX::SLIP) = q_cov_slip; - 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); + + // 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");