Skip to content

Commit

Permalink
chore: explanations of Kalman filter matrices
Browse files Browse the repository at this point in the history 
Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
  • Loading branch information
@technolojin
technolojin committed Jan 15, 2024
1 parent fd75129 commit 6b8a8c6
Show file tree
Hide file tree
Showing 4 changed files with 34 additions and 28 deletions.
2 changes: 1 addition & 1 deletion perception/multi_object_tracker/src/data_association/data_association.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down
20 changes: 11 additions & 9 deletions perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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]
Expand All @@ -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;
Expand All @@ -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) {
Expand Down Expand Up @@ -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));
Expand All @@ -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)
Expand All @@ -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;
Expand All @@ -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;
Expand Down Expand Up @@ -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);

Check warning on line 282 in perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Large Method 

BicycleTracker::predict has 71 lines, threshold = 70. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.

if (!ekf.predict(X_next_t, A, Q)) {
RCLCPP_WARN(logger_, "Cannot predict");
Expand Down
20 changes: 11 additions & 9 deletions perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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]
Expand Down Expand Up @@ -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;
Expand All @@ -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));
Expand Down Expand Up @@ -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));
Expand All @@ -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)
Expand All @@ -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;
Expand All @@ -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;
Expand Down Expand Up @@ -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);

Check warning on line 297 in perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Large Method 

BigVehicleTracker::predict has 71 lines, threshold = 70. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.

if (!ekf.predict(X_next_t, A, Q)) {
RCLCPP_WARN(logger_, "Cannot predict");
Expand Down
20 changes: 11 additions & 9 deletions perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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]
Expand Down Expand Up @@ -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;
Expand All @@ -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));
Expand Down Expand Up @@ -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));
Expand All @@ -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)
Expand All @@ -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;
Expand All @@ -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;
Expand Down Expand Up @@ -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);

Check warning on line 297 in perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Large Method 

NormalVehicleTracker::predict has 71 lines, threshold = 70. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.

if (!ekf.predict(X_next_t, A, Q)) {
RCLCPP_WARN(logger_, "Cannot predict");
Expand Down

0 comments on commit 6b8a8c6

Please sign in to comment.