Skip to content

Commit dd1d24a

Browse files
technolojintechnolojin
committed
feat: parameter tuning
Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
1 parent af6fac8 commit dd1d24a

File tree

3 files changed

+40
-37
lines changed

3 files changed

+40
-37
lines changed

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

+12-11
Original file line numberDiff line numberDiff line change
@@ -53,9 +53,9 @@ BicycleTracker::BicycleTracker(
5353
object_ = object;
5454

5555
// Initialize parameters
56-
// measurement noise covariance
57-
float r_stddev_x = 0.6; // [m]
58-
float r_stddev_y = 0.4; // [m]
56+
// measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty
57+
float r_stddev_x = 0.5; // in vehicle coordinate [m]
58+
float r_stddev_y = 0.4; // in vehicle coordinate [m]
5959
float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad]
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);
@@ -72,8 +72,8 @@ BicycleTracker::BicycleTracker(
7272
ekf_params_.p0_cov_vel = std::pow(p0_stddev_vel, 2.0);
7373
ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0);
7474
// process noise covariance
75-
ekf_params_.q_stddev_acc_long = 9.8 * 0.3; // [m/(s*s)] uncertain longitudinal acceleration
76-
ekf_params_.q_stddev_acc_lat = 9.8 * 0.1; // [m/(s*s)] uncertain lateral acceleration
75+
ekf_params_.q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration
76+
ekf_params_.q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration
7777
ekf_params_.q_stddev_yaw_rate_min =
7878
tier4_autoware_utils::deg2rad(5.0); // [rad/s] uncertain yaw change rate
7979
ekf_params_.q_stddev_yaw_rate_max =
@@ -204,23 +204,24 @@ bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const
204204
ekf.getX(X_t);
205205
const double cos_yaw = std::cos(X_t(IDX::YAW) + X_t(IDX::SLIP));
206206
const double sin_yaw = std::sin(X_t(IDX::YAW) + X_t(IDX::SLIP));
207+
const double vel = X_t(IDX::VEL);
207208
const double cos_slip = std::cos(X_t(IDX::SLIP));
208209
const double sin_slip = std::sin(X_t(IDX::SLIP));
209-
const double vel = X_t(IDX::VEL);
210+
211+
double w = vel * sin_slip / lr_;
210212
const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW));
211-
const double w = vel * sin_slip / lr_;
212213
const double w_dtdt = w * dt * dt;
213214
const double vv_dtdt__lr = vel * vel * dt * dt / lr_;
214215

215216
// Predict state vector X t+1
216217
Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state
217218
X_next_t(IDX::X) =
218-
X_t(IDX::X) + vel * cos_yaw * dt - 0.5 * vel * sin_slip * w_dtdt; // dx = v * cos(yaw)
219+
X_t(IDX::X) + vel * cos_yaw * dt - 0.5 * vel * sin_slip * w_dtdt; // dx = v * cos(yaw) * dt
219220
X_next_t(IDX::Y) =
220-
X_t(IDX::Y) + vel * sin_yaw * dt + 0.5 * vel * cos_slip * w_dtdt; // dy = v * sin(yaw)
221-
X_next_t(IDX::YAW) = X_t(IDX::YAW) + vel / lr_ * sin_slip * dt; // dyaw = omega
221+
X_t(IDX::Y) + vel * sin_yaw * dt + 0.5 * vel * cos_slip * w_dtdt; // dy = v * sin(yaw) * dt
222+
X_next_t(IDX::YAW) = X_t(IDX::YAW) + w * dt; // d(yaw) = w * dt
222223
X_next_t(IDX::VEL) = X_t(IDX::VEL);
223-
X_next_t(IDX::SLIP) = X_t(IDX::SLIP);
224+
X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // slip_angle = asin(lr * w / v)
224225

225226
// State transition matrix A
226227
Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x);

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

+14-13
Original file line numberDiff line numberDiff line change
@@ -52,9 +52,9 @@ BigVehicleTracker::BigVehicleTracker(
5252
object_ = object;
5353

5454
// Initialize parameters
55-
// measurement noise covariance
56-
float r_stddev_x = 1.5; // [m]
57-
float r_stddev_y = 0.3; // [m]
55+
// measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty
56+
float r_stddev_x = 0.5; // in vehicle coordinate [m]
57+
float r_stddev_y = 0.4; // in vehicle coordinate [m]
5858
float r_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad]
5959
float r_stddev_vel = 1.0; // [m/s]
6060
ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0);
@@ -73,8 +73,8 @@ BigVehicleTracker::BigVehicleTracker(
7373
ekf_params_.p0_cov_vel = std::pow(p0_stddev_vel, 2.0);
7474
ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0);
7575
// process noise covariance
76-
ekf_params_.q_stddev_acc_long = 9.8 * 0.3; // [m/(s*s)] uncertain longitudinal acceleration
77-
ekf_params_.q_stddev_acc_lat = 9.8 * 0.1; // [m/(s*s)] uncertain lateral acceleration
76+
ekf_params_.q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration
77+
ekf_params_.q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration
7878
ekf_params_.q_stddev_yaw_rate_min =
7979
tier4_autoware_utils::deg2rad(1.5); // [rad/s] uncertain yaw change rate
8080
ekf_params_.q_stddev_yaw_rate_max =
@@ -219,23 +219,24 @@ bool BigVehicleTracker::predict(const double dt, KalmanFilter & ekf) const
219219
ekf.getX(X_t);
220220
const double cos_yaw = std::cos(X_t(IDX::YAW) + X_t(IDX::SLIP));
221221
const double sin_yaw = std::sin(X_t(IDX::YAW) + X_t(IDX::SLIP));
222+
const double vel = X_t(IDX::VEL);
222223
const double cos_slip = std::cos(X_t(IDX::SLIP));
223224
const double sin_slip = std::sin(X_t(IDX::SLIP));
224-
const double vel = X_t(IDX::VEL);
225+
226+
double w = vel * sin_slip / lr_;
225227
const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW));
226-
const double w = vel * sin_slip / lr_;
227228
const double w_dtdt = w * dt * dt;
228229
const double vv_dtdt__lr = vel * vel * dt * dt / lr_;
229230

230231
// Predict state vector X t+1
231232
Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state
232233
X_next_t(IDX::X) =
233-
X_t(IDX::X) + vel * cos_yaw * dt - 0.5 * vel * sin_slip * w_dtdt; // dx = v * cos(yaw)
234+
X_t(IDX::X) + vel * cos_yaw * dt - 0.5 * vel * sin_slip * w_dtdt; // dx = v * cos(yaw) * dt
234235
X_next_t(IDX::Y) =
235-
X_t(IDX::Y) + vel * sin_yaw * dt + 0.5 * vel * cos_slip * w_dtdt; // dy = v * sin(yaw)
236-
X_next_t(IDX::YAW) = X_t(IDX::YAW) + vel / lr_ * sin_slip * dt; // dyaw = omega
236+
X_t(IDX::Y) + vel * sin_yaw * dt + 0.5 * vel * cos_slip * w_dtdt; // dy = v * sin(yaw) * dt
237+
X_next_t(IDX::YAW) = X_t(IDX::YAW) + w * dt; // d(yaw) = w * dt
237238
X_next_t(IDX::VEL) = X_t(IDX::VEL);
238-
X_next_t(IDX::SLIP) = X_t(IDX::SLIP);
239+
X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // slip_angle = asin(lr * w / v)
239240

240241
// State transition matrix A
241242
Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x);
@@ -317,8 +318,8 @@ bool BigVehicleTracker::measureWithPose(
317318
const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification);
318319

319320
if (label == Label::CAR) {
320-
constexpr float r_stddev_x = 8.0; // [m]
321-
constexpr float r_stddev_y = 0.8; // [m]
321+
constexpr float r_stddev_x = 2.0; // [m]
322+
constexpr float r_stddev_y = 2.0; // [m]
322323
r_cov_x = std::pow(r_stddev_x, 2.0);
323324
r_cov_y = std::pow(r_stddev_y, 2.0);
324325
} else if (utils::isLargeVehicleLabel(label)) {

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

+14-13
Original file line numberDiff line numberDiff line change
@@ -52,9 +52,9 @@ NormalVehicleTracker::NormalVehicleTracker(
5252
object_ = object;
5353

5454
// Initialize parameters
55-
// measurement noise covariance
56-
float r_stddev_x = 1.0; // in object coordinate [m]
57-
float r_stddev_y = 0.3; // in object coordinate [m]
55+
// measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty
56+
float r_stddev_x = 0.5; // in vehicle coordinate [m]
57+
float r_stddev_y = 0.4; // in vehicle coordinate [m]
5858
float r_stddev_yaw = tier4_autoware_utils::deg2rad(20); // in map coordinate [rad]
5959
float r_stddev_vel = 1.0; // in object coordinate [m/s]
6060
ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0);
@@ -73,8 +73,8 @@ NormalVehicleTracker::NormalVehicleTracker(
7373
ekf_params_.p0_cov_vel = std::pow(p0_stddev_vel, 2.0);
7474
ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0);
7575
// process noise covariance
76-
ekf_params_.q_stddev_acc_long = 9.8 * 0.3; // [m/(s*s)] uncertain longitudinal acceleration
77-
ekf_params_.q_stddev_acc_lat = 9.8 * 0.1; // [m/(s*s)] uncertain lateral acceleration
76+
ekf_params_.q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration
77+
ekf_params_.q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration
7878
ekf_params_.q_stddev_yaw_rate_min =
7979
tier4_autoware_utils::deg2rad(1.5); // [rad/s] uncertain yaw change rate
8080
ekf_params_.q_stddev_yaw_rate_max =
@@ -219,23 +219,24 @@ bool NormalVehicleTracker::predict(const double dt, KalmanFilter & ekf) const
219219
ekf.getX(X_t);
220220
const double cos_yaw = std::cos(X_t(IDX::YAW) + X_t(IDX::SLIP));
221221
const double sin_yaw = std::sin(X_t(IDX::YAW) + X_t(IDX::SLIP));
222+
const double vel = X_t(IDX::VEL);
222223
const double cos_slip = std::cos(X_t(IDX::SLIP));
223224
const double sin_slip = std::sin(X_t(IDX::SLIP));
224-
const double vel = X_t(IDX::VEL);
225+
226+
double w = vel * sin_slip / lr_;
225227
const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW));
226-
const double w = vel * sin_slip / lr_;
227228
const double w_dtdt = w * dt * dt;
228229
const double vv_dtdt__lr = vel * vel * dt * dt / lr_;
229230

230231
// Predict state vector X t+1
231232
Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state
232233
X_next_t(IDX::X) =
233-
X_t(IDX::X) + vel * cos_yaw * dt - 0.5 * vel * sin_slip * w_dtdt; // dx = v * cos(yaw)
234+
X_t(IDX::X) + vel * cos_yaw * dt - 0.5 * vel * sin_slip * w_dtdt; // dx = v * cos(yaw) * dt
234235
X_next_t(IDX::Y) =
235-
X_t(IDX::Y) + vel * sin_yaw * dt + 0.5 * vel * cos_slip * w_dtdt; // dy = v * sin(yaw)
236-
X_next_t(IDX::YAW) = X_t(IDX::YAW) + vel / lr_ * sin_slip * dt; // dyaw = omega
236+
X_t(IDX::Y) + vel * sin_yaw * dt + 0.5 * vel * cos_slip * w_dtdt; // dy = v * sin(yaw) * dt
237+
X_next_t(IDX::YAW) = X_t(IDX::YAW) + w * dt; // d(yaw) = w * dt
237238
X_next_t(IDX::VEL) = X_t(IDX::VEL);
238-
X_next_t(IDX::SLIP) = X_t(IDX::SLIP);
239+
X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // slip_angle = asin(lr * w / v)
239240

240241
// State transition matrix A
241242
Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x);
@@ -320,8 +321,8 @@ bool NormalVehicleTracker::measureWithPose(
320321
r_cov_x = ekf_params_.r_cov_x;
321322
r_cov_y = ekf_params_.r_cov_y;
322323
} else if (utils::isLargeVehicleLabel(label)) {
323-
constexpr float r_stddev_x = 8.0; // [m]
324-
constexpr float r_stddev_y = 0.8; // [m]
324+
constexpr float r_stddev_x = 2.0; // [m]
325+
constexpr float r_stddev_y = 2.0; // [m]
325326
r_cov_x = std::pow(r_stddev_x, 2.0);
326327
r_cov_y = std::pow(r_stddev_y, 2.0);
327328
} else {

0 commit comments

Comments
 (0)