Skip to content

Commit 99f93f2

Browse files
technolojintechnolojin
committed
chore: explanations of Kalman filter matrices
Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
1 parent 62ddd5f commit 99f93f2

File tree

4 files changed

+34
-28
lines changed

4 files changed

+34
-28
lines changed

‎perception/multi_object_tracker/src/data_association/data_association.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -201,7 +201,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix(
201201
measurement_object.kinematics.pose_with_covariance.pose.position,
202202
tracked_object.kinematics.pose_with_covariance.pose.position,
203203
getXYCovariance(tracked_object.kinematics.pose_with_covariance));
204-
if (2.448 /*95%*/ <= mahalanobis_dist) passed_gate = true;
204+
if (2.448 /*95%*/ <= mahalanobis_dist) passed_gate = true; // gate disabled
205205
}
206206
// 2d iou gate
207207
if (passed_gate) {

‎perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp

+11-9
Original file line numberDiff line numberDiff line change
@@ -60,7 +60,7 @@ BicycleTracker::BicycleTracker(
6060
ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0);
6161
ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0);
6262
ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0);
63-
// initial covariance
63+
// initial state covariance
6464
float p0_stddev_x = 0.8; // [m]
6565
float p0_stddev_y = 0.5; // [m]
6666
float p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad]
@@ -87,7 +87,7 @@ BicycleTracker::BicycleTracker(
8787
max_vel_ = tier4_autoware_utils::kmph2mps(100); // [m/s]
8888
max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad/s]
8989

90-
// initialize X matrix
90+
// initialize state vector X
9191
Eigen::MatrixXd X(ekf_params_.dim_x, 1);
9292
X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x;
9393
X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y;
@@ -99,7 +99,7 @@ BicycleTracker::BicycleTracker(
9999
}
100100
X(IDX::SLIP) = 0.0;
101101

102-
// initialize P matrix
102+
// initialize state covariance matrix P
103103
Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x);
104104

105105
if (!object.kinematics.has_position_covariance) {
@@ -197,7 +197,7 @@ bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const
197197
*
198198
*/
199199

200-
// X t
200+
// Current state vector X t
201201
Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state
202202
ekf.getX(X_t);
203203
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
210210
const double w_dtdt = w * dt * dt;
211211
const double vv_dtdt__lr = vel * vel * dt * dt / lr_;
212212

213-
// X t+1
213+
// Predict state vector X t+1
214214
Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state
215215
X_next_t(IDX::X) =
216216
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
220220
X_next_t(IDX::VEL) = X_t(IDX::VEL);
221221
X_next_t(IDX::SLIP) = X_t(IDX::SLIP);
222222

223-
// A
223+
// State transition matrix A
224224
Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x);
225225
A(IDX::X, IDX::YAW) = -vel * sin_yaw * dt - 0.5 * vel * cos_yaw * w_dtdt;
226226
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
233233
A(IDX::YAW, IDX::VEL) = 1.0 / lr_ * sin_slip * dt;
234234
A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt;
235235

236-
// Q
236+
// Process noise covariance Q
237237
float q_stddev_yaw_rate{0.0};
238238
if (vel <= 0.01) {
239239
q_stddev_yaw_rate = ekf_params_.q_stddev_yaw_rate_min;
@@ -276,8 +276,10 @@ bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const
276276
Q(IDX::YAW, IDX::YAW) = q_cov_yaw;
277277
Q(IDX::VEL, IDX::VEL) = q_cov_vel;
278278
Q(IDX::SLIP, IDX::SLIP) = q_cov_slip;
279-
Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x);
280-
Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1);
279+
280+
// control-input model B and control-input u are not used
281+
// Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x);
282+
// Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1);
281283

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

‎perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp

+11-9
Original file line numberDiff line numberDiff line change
@@ -61,7 +61,7 @@ BigVehicleTracker::BigVehicleTracker(
6161
ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0);
6262
ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0);
6363
ekf_params_.r_cov_vel = std::pow(r_stddev_vel, 2.0);
64-
// initial covariance
64+
// initial state covariance
6565
float p0_stddev_x = 1.5; // [m]
6666
float p0_stddev_y = 0.5; // [m]
6767
float p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad]
@@ -89,7 +89,7 @@ BigVehicleTracker::BigVehicleTracker(
8989
max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad]
9090
velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s]
9191

92-
// initialize X matrix
92+
// initialize state vector X
9393
Eigen::MatrixXd X(ekf_params_.dim_x, 1);
9494
X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x;
9595
X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y;
@@ -102,7 +102,7 @@ BigVehicleTracker::BigVehicleTracker(
102102
X(IDX::VEL) = 0.0;
103103
}
104104

105-
// initialize P matrix
105+
// initialize state covariance matrix P
106106
Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x);
107107
if (!object.kinematics.has_position_covariance) {
108108
const double cos_yaw = std::cos(X(IDX::YAW));
@@ -212,7 +212,7 @@ bool BigVehicleTracker::predict(const double dt, KalmanFilter & ekf) const
212212
*
213213
*/
214214

215-
// X t
215+
// Current state vector X t
216216
Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state
217217
ekf.getX(X_t);
218218
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
225225
const double w_dtdt = w * dt * dt;
226226
const double vv_dtdt__lr = vel * vel * dt * dt / lr_;
227227

228-
// X t+1
228+
// Predict state vector X t+1
229229
Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state
230230
X_next_t(IDX::X) =
231231
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
235235
X_next_t(IDX::VEL) = X_t(IDX::VEL);
236236
X_next_t(IDX::SLIP) = X_t(IDX::SLIP);
237237

238-
// A
238+
// State transition matrix A
239239
Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x);
240240
A(IDX::X, IDX::YAW) = -vel * sin_yaw * dt - 0.5 * vel * cos_yaw * w_dtdt;
241241
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
248248
A(IDX::YAW, IDX::VEL) = 1.0 / lr_ * sin_slip * dt;
249249
A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt;
250250

251-
// Q
251+
// Process noise covariance Q
252252
float q_stddev_yaw_rate{0.0};
253253
if (vel <= 0.01) {
254254
q_stddev_yaw_rate = ekf_params_.q_stddev_yaw_rate_min;
@@ -291,8 +291,10 @@ bool BigVehicleTracker::predict(const double dt, KalmanFilter & ekf) const
291291
Q(IDX::YAW, IDX::YAW) = q_cov_yaw;
292292
Q(IDX::VEL, IDX::VEL) = q_cov_vel;
293293
Q(IDX::SLIP, IDX::SLIP) = q_cov_slip;
294-
Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x);
295-
Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1);
294+
295+
// control-input model B and control-input u are not used
296+
// Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x);
297+
// Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1);
296298

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

‎perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp

+11-9
Original file line numberDiff line numberDiff line change
@@ -61,7 +61,7 @@ NormalVehicleTracker::NormalVehicleTracker(
6161
ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0);
6262
ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0);
6363
ekf_params_.r_cov_vel = std::pow(r_stddev_vel, 2.0);
64-
// initial covariance
64+
// initial state covariance
6565
float p0_stddev_x = 1.0; // in object coordinate [m]
6666
float p0_stddev_y = 0.3; // in object coordinate [m]
6767
float p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad]
@@ -89,7 +89,7 @@ NormalVehicleTracker::NormalVehicleTracker(
8989
max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad/s]
9090
velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s]
9191

92-
// initialize X matrix
92+
// initialize state vector X
9393
Eigen::MatrixXd X(ekf_params_.dim_x, 1);
9494
X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x;
9595
X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y;
@@ -101,7 +101,7 @@ NormalVehicleTracker::NormalVehicleTracker(
101101
}
102102
X(IDX::SLIP) = 0.0;
103103

104-
// initialize P matrix
104+
// initialize state covariance matrix P
105105
Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x);
106106
if (!object.kinematics.has_position_covariance) {
107107
const double cos_yaw = std::cos(X(IDX::YAW));
@@ -212,7 +212,7 @@ bool NormalVehicleTracker::predict(const double dt, KalmanFilter & ekf) const
212212
*
213213
*/
214214

215-
// X t
215+
// Current state vector X t
216216
Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state
217217
ekf.getX(X_t);
218218
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
225225
const double w_dtdt = w * dt * dt;
226226
const double vv_dtdt__lr = vel * vel * dt * dt / lr_;
227227

228-
// X t+1
228+
// Predict state vector X t+1
229229
Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state
230230
X_next_t(IDX::X) =
231231
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
235235
X_next_t(IDX::VEL) = X_t(IDX::VEL);
236236
X_next_t(IDX::SLIP) = X_t(IDX::SLIP);
237237

238-
// A
238+
// State transition matrix A
239239
Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x);
240240
A(IDX::X, IDX::YAW) = -vel * sin_yaw * dt - 0.5 * vel * cos_yaw * w_dtdt;
241241
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
248248
A(IDX::YAW, IDX::VEL) = 1.0 / lr_ * sin_slip * dt;
249249
A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt;
250250

251-
// Q
251+
// Process noise covariance Q
252252
float q_stddev_yaw_rate{0.0};
253253
if (vel <= 0.01) {
254254
q_stddev_yaw_rate = ekf_params_.q_stddev_yaw_rate_min;
@@ -291,8 +291,10 @@ bool NormalVehicleTracker::predict(const double dt, KalmanFilter & ekf) const
291291
Q(IDX::YAW, IDX::YAW) = q_cov_yaw;
292292
Q(IDX::VEL, IDX::VEL) = q_cov_vel;
293293
Q(IDX::SLIP, IDX::SLIP) = q_cov_slip;
294-
Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x);
295-
Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1);
294+
295+
// control-input model B and control-input u are not used
296+
// Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x);
297+
// Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1);
296298

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

0 commit comments

Comments
 (0)