Skip to content

Commit 62ddd5f

Browse files
technolojintechnolojin
committed
fix: twist covariance matrix
Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
1 parent 8fb2748 commit 62ddd5f

File tree

3 files changed

+133
-56
lines changed

3 files changed

+133
-56
lines changed

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

+45-19
Original file line numberDiff line numberDiff line change
@@ -63,9 +63,9 @@ BicycleTracker::BicycleTracker(
6363
// initial covariance
6464
float p0_stddev_x = 0.8; // [m]
6565
float p0_stddev_y = 0.5; // [m]
66-
float p0_stddev_yaw = tier4_autoware_utils::deg2rad(1000); // [rad]
67-
float p0_stddev_vel = tier4_autoware_utils::kmph2mps(1000); // [m/s]
68-
float p0_stddev_slip = tier4_autoware_utils::deg2rad(10); // [rad/s]
66+
float p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad]
67+
float p0_stddev_vel = tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s]
68+
float p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s]
6969
ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0);
7070
ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0);
7171
ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0);
@@ -79,9 +79,9 @@ BicycleTracker::BicycleTracker(
7979
ekf_params_.q_stddev_yaw_rate_max =
8080
tier4_autoware_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate
8181
ekf_params_.q_stddev_slip_rate_min =
82-
tier4_autoware_utils::deg2rad(2.0); // [rad/s] uncertain slip angle change rate
82+
tier4_autoware_utils::deg2rad(1.0); // [rad/s] uncertain slip angle change rate
8383
ekf_params_.q_stddev_slip_rate_max =
84-
tier4_autoware_utils::deg2rad(12.0); // [rad/s] uncertain slip angle change rate
84+
tier4_autoware_utils::deg2rad(7.0); // [rad/s] uncertain slip angle change rate
8585
ekf_params_.q_max_slip_angle = tier4_autoware_utils::deg2rad(30); // [rad] max slip angle
8686
// limitations
8787
max_vel_ = tier4_autoware_utils::kmph2mps(100); // [m/s]
@@ -247,19 +247,24 @@ bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const
247247
q_stddev_yaw_rate = std::min(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_max);
248248
q_stddev_yaw_rate = std::max(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_min);
249249
}
250-
float q_stddev_slip_rate{0.0};
250+
float q_cov_slip_rate{0.0};
251251
if (vel <= 0.01) {
252-
q_stddev_slip_rate = ekf_params_.q_stddev_slip_rate_min;
252+
q_cov_slip_rate = ekf_params_.q_stddev_slip_rate_min * ekf_params_.q_stddev_slip_rate_min;
253253
} else {
254-
q_stddev_slip_rate = std::abs(sin_slip * ekf_params_.q_stddev_acc_lat / vel);
255-
q_stddev_slip_rate = std::min(q_stddev_slip_rate, ekf_params_.q_stddev_slip_rate_max);
256-
q_stddev_slip_rate = std::max(q_stddev_slip_rate, ekf_params_.q_stddev_slip_rate_min);
254+
// sin(slip) = w*l_r/v
255+
// d(slip)/dt = - w*l_r/v^2 * d(v)/dt + l_r/v * d(w)/dt
256+
q_cov_slip_rate = std::pow(sin_slip * ekf_params_.q_stddev_acc_lat / vel, 2) +
257+
std::pow(q_stddev_yaw_rate * lr_ / vel, 2);
258+
q_cov_slip_rate = std::min(
259+
q_cov_slip_rate, ekf_params_.q_stddev_slip_rate_max * ekf_params_.q_stddev_slip_rate_max);
260+
q_cov_slip_rate = std::max(
261+
q_cov_slip_rate, ekf_params_.q_stddev_slip_rate_min * ekf_params_.q_stddev_slip_rate_min);
257262
}
258263
const float q_cov_x = std::pow(0.5 * ekf_params_.q_stddev_acc_long * dt * dt, 2.0);
259264
const float q_cov_y = std::pow(0.5 * ekf_params_.q_stddev_acc_lat * dt * dt, 2.0);
260265
const float q_cov_yaw = std::pow(q_stddev_yaw_rate * dt, 2.0);
261266
const float q_cov_vel = std::pow(ekf_params_.q_stddev_acc_long * dt, 2.0);
262-
const float q_cov_slip = std::pow(q_stddev_slip_rate * dt, 2.0);
267+
const float q_cov_slip = q_cov_slip_rate * dt * dt;
263268

264269
Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x);
265270
// Rotate the covariance matrix according to the vehicle yaw
@@ -492,20 +497,41 @@ bool BicycleTracker::getTrackedObject(
492497
twist_with_cov.twist.linear.x = X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP));
493498
twist_with_cov.twist.linear.y = X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP));
494499
twist_with_cov.twist.angular.z =
495-
X_t(IDX::VEL) / lr_ * std::sin(X_t(IDX::SLIP)); // yaw_rate = vel_k / l_r * sin(slip_k)
496-
// twist covariance
497-
constexpr double vy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative
500+
X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)) / lr_; // yaw_rate = vel_k * sin(slip_k) / l_r
501+
/* twist covariance
502+
* convert covariance from velocity, slip angle to vx, vy, and yaw angle
503+
*
504+
* vx = vel * cos(slip)
505+
* vy = vel * sin(slip)
506+
* wz = vel * sin(slip) / l_r = vy / l_r
507+
*
508+
*/
509+
498510
constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative
499511
constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative
500512
constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative
501-
twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VEL, IDX::VEL);
502-
twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = vy_cov;
513+
514+
Eigen::MatrixXd cov_jacob(3, 2);
515+
cov_jacob << std::cos(X_t(IDX::SLIP)), -X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)),
516+
std::sin(X_t(IDX::SLIP)), X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)),
517+
std::sin(X_t(IDX::SLIP)) / lr_, X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)) / lr_;
518+
Eigen::MatrixXd cov_twist(2, 2);
519+
cov_twist << P(IDX::VEL, IDX::VEL), P(IDX::VEL, IDX::SLIP), P(IDX::SLIP, IDX::VEL),
520+
P(IDX::SLIP, IDX::SLIP);
521+
Eigen::MatrixXd twist_cov_mat = cov_jacob * cov_twist * cov_jacob.transpose();
522+
523+
twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = twist_cov_mat(0, 0);
524+
twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = twist_cov_mat(0, 1);
525+
twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = twist_cov_mat(0, 2);
526+
twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = twist_cov_mat(1, 0);
527+
twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = twist_cov_mat(1, 1);
528+
twist_with_cov.covariance[utils::MSG_COV_IDX::Y_YAW] = twist_cov_mat(1, 2);
529+
twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = twist_cov_mat(2, 0);
530+
twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_Y] = twist_cov_mat(2, 1);
531+
twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = twist_cov_mat(2, 2);
503532
twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov;
504-
twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = P(IDX::VEL, IDX::SLIP);
505-
twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = P(IDX::SLIP, IDX::VEL);
506533
twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov;
507534
twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov;
508-
twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::SLIP, IDX::SLIP);
509535

510536
// set shape
511537
object.shape.dimensions.x = bounding_box_.length;

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

+44-19
Original file line numberDiff line numberDiff line change
@@ -64,9 +64,9 @@ BigVehicleTracker::BigVehicleTracker(
6464
// initial covariance
6565
float p0_stddev_x = 1.5; // [m]
6666
float p0_stddev_y = 0.5; // [m]
67-
float p0_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad]
68-
float p0_stddev_vel = tier4_autoware_utils::kmph2mps(1000); // [m/s]
69-
float p0_stddev_slip = tier4_autoware_utils::deg2rad(10); // [rad/s]
67+
float p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad]
68+
float p0_stddev_vel = tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s]
69+
float p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s]
7070
ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0);
7171
ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0);
7272
ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0);
@@ -80,9 +80,9 @@ BigVehicleTracker::BigVehicleTracker(
8080
ekf_params_.q_stddev_yaw_rate_max =
8181
tier4_autoware_utils::deg2rad(13.0); // [rad/s] uncertain yaw change rate
8282
ekf_params_.q_stddev_slip_rate_min =
83-
tier4_autoware_utils::deg2rad(0.6); // [rad/s] uncertain slip angle change rate
83+
tier4_autoware_utils::deg2rad(0.2); // [rad/s] uncertain slip angle change rate
8484
ekf_params_.q_stddev_slip_rate_max =
85-
tier4_autoware_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate
85+
tier4_autoware_utils::deg2rad(5.0); // [rad/s] uncertain slip angle change rate
8686
ekf_params_.q_max_slip_angle = tier4_autoware_utils::deg2rad(30); // [rad] max slip angle
8787
// limitations
8888
max_vel_ = tier4_autoware_utils::kmph2mps(100); // [m/s]
@@ -262,19 +262,24 @@ bool BigVehicleTracker::predict(const double dt, KalmanFilter & ekf) const
262262
q_stddev_yaw_rate = std::min(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_max);
263263
q_stddev_yaw_rate = std::max(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_min);
264264
}
265-
float q_stddev_slip_rate{0.0};
265+
float q_cov_slip_rate{0.0};
266266
if (vel <= 0.01) {
267-
q_stddev_slip_rate = ekf_params_.q_stddev_slip_rate_min;
267+
q_cov_slip_rate = ekf_params_.q_stddev_slip_rate_min * ekf_params_.q_stddev_slip_rate_min;
268268
} else {
269-
q_stddev_slip_rate = std::abs(sin_slip * ekf_params_.q_stddev_acc_lat / vel);
270-
q_stddev_slip_rate = std::min(q_stddev_slip_rate, ekf_params_.q_stddev_slip_rate_max);
271-
q_stddev_slip_rate = std::max(q_stddev_slip_rate, ekf_params_.q_stddev_slip_rate_min);
269+
// sin(slip) = w*l_r/v
270+
// d(slip)/dt = - w*l_r/v^2 * d(v)/dt + l_r/v * d(w)/dt
271+
q_cov_slip_rate = std::pow(sin_slip * ekf_params_.q_stddev_acc_lat / vel, 2) +
272+
std::pow(q_stddev_yaw_rate * lr_ / vel, 2);
273+
q_cov_slip_rate = std::min(
274+
q_cov_slip_rate, ekf_params_.q_stddev_slip_rate_max * ekf_params_.q_stddev_slip_rate_max);
275+
q_cov_slip_rate = std::max(
276+
q_cov_slip_rate, ekf_params_.q_stddev_slip_rate_min * ekf_params_.q_stddev_slip_rate_min);
272277
}
273278
const float q_cov_x = std::pow(0.5 * ekf_params_.q_stddev_acc_long * dt * dt, 2.0);
274279
const float q_cov_y = std::pow(0.5 * ekf_params_.q_stddev_acc_lat * dt * dt, 2.0);
275280
const float q_cov_yaw = std::pow(q_stddev_yaw_rate * dt, 2.0);
276281
const float q_cov_vel = std::pow(ekf_params_.q_stddev_acc_long * dt, 2.0);
277-
const float q_cov_slip = std::pow(q_stddev_slip_rate * dt, 2.0);
282+
const float q_cov_slip = q_cov_slip_rate * dt * dt;
278283

279284
Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x);
280285
// Rotate the covariance matrix according to the vehicle yaw
@@ -544,20 +549,40 @@ bool BigVehicleTracker::getTrackedObject(
544549
twist_with_cov.twist.linear.x = X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP));
545550
twist_with_cov.twist.linear.y = X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP));
546551
twist_with_cov.twist.angular.z =
547-
X_t(IDX::VEL) / lr_ * std::sin(X_t(IDX::SLIP)); // yaw_rate = vel_k / l_r * sin(slip_k)
548-
// twist covariance
549-
constexpr double vy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative
552+
X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)) / lr_; // yaw_rate = vel_k * sin(slip_k) / l_r
553+
/* twist covariance
554+
* convert covariance from velocity, slip angle to vx, vy, and yaw angle
555+
*
556+
* vx = vel * cos(slip)
557+
* vy = vel * sin(slip)
558+
* wz = vel * sin(slip) / l_r = vy / l_r
559+
*
560+
*/
550561
constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative
551562
constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative
552563
constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative
553-
twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VEL, IDX::VEL);
554-
twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = vy_cov;
564+
565+
Eigen::MatrixXd cov_jacob(3, 2);
566+
cov_jacob << std::cos(X_t(IDX::SLIP)), -X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)),
567+
std::sin(X_t(IDX::SLIP)), X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)),
568+
std::sin(X_t(IDX::SLIP)) / lr_, X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)) / lr_;
569+
Eigen::MatrixXd cov_twist(2, 2);
570+
cov_twist << P(IDX::VEL, IDX::VEL), P(IDX::VEL, IDX::SLIP), P(IDX::SLIP, IDX::VEL),
571+
P(IDX::SLIP, IDX::SLIP);
572+
Eigen::MatrixXd twist_cov_mat = cov_jacob * cov_twist * cov_jacob.transpose();
573+
574+
twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = twist_cov_mat(0, 0);
575+
twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = twist_cov_mat(0, 1);
576+
twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = twist_cov_mat(0, 2);
577+
twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = twist_cov_mat(1, 0);
578+
twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = twist_cov_mat(1, 1);
579+
twist_with_cov.covariance[utils::MSG_COV_IDX::Y_YAW] = twist_cov_mat(1, 2);
580+
twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = twist_cov_mat(2, 0);
581+
twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_Y] = twist_cov_mat(2, 1);
582+
twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = twist_cov_mat(2, 2);
555583
twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov;
556-
twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = P(IDX::VEL, IDX::SLIP);
557-
twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = P(IDX::SLIP, IDX::VEL);
558584
twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov;
559585
twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov;
560-
twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::SLIP, IDX::SLIP);
561586

562587
// set shape
563588
object.shape.dimensions.x = bounding_box_.length;

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

+44-18
Original file line numberDiff line numberDiff line change
@@ -64,9 +64,9 @@ NormalVehicleTracker::NormalVehicleTracker(
6464
// initial covariance
6565
float p0_stddev_x = 1.0; // in object coordinate [m]
6666
float p0_stddev_y = 0.3; // in object coordinate [m]
67-
float p0_stddev_yaw = tier4_autoware_utils::deg2rad(30); // in map coordinate [rad]
67+
float p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad]
6868
float p0_stddev_vel = tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s]
69-
float p0_stddev_slip = tier4_autoware_utils::deg2rad(10); // in object coordinate [rad/s]
69+
float p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s]
7070
ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0);
7171
ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0);
7272
ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0);
@@ -80,9 +80,9 @@ NormalVehicleTracker::NormalVehicleTracker(
8080
ekf_params_.q_stddev_yaw_rate_max =
8181
tier4_autoware_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate
8282
ekf_params_.q_stddev_slip_rate_min =
83-
tier4_autoware_utils::deg2rad(1.1); // [rad/s] uncertain slip angle change rate
83+
tier4_autoware_utils::deg2rad(0.3); // [rad/s] uncertain slip angle change rate
8484
ekf_params_.q_stddev_slip_rate_max =
85-
tier4_autoware_utils::deg2rad(12.0); // [rad/s] uncertain slip angle change rate
85+
tier4_autoware_utils::deg2rad(5.0); // [rad/s] uncertain slip angle change rate
8686
ekf_params_.q_max_slip_angle = tier4_autoware_utils::deg2rad(30); // [rad] max slip angle
8787
// limitations
8888
max_vel_ = tier4_autoware_utils::kmph2mps(100); // [m/s]
@@ -262,19 +262,24 @@ bool NormalVehicleTracker::predict(const double dt, KalmanFilter & ekf) const
262262
q_stddev_yaw_rate = std::min(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_max);
263263
q_stddev_yaw_rate = std::max(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_min);
264264
}
265-
float q_stddev_slip_rate{0.0};
265+
float q_cov_slip_rate{0.0};
266266
if (vel <= 0.01) {
267-
q_stddev_slip_rate = ekf_params_.q_stddev_slip_rate_min;
267+
q_cov_slip_rate = ekf_params_.q_stddev_slip_rate_min * ekf_params_.q_stddev_slip_rate_min;
268268
} else {
269-
q_stddev_slip_rate = std::abs(sin_slip * ekf_params_.q_stddev_acc_lat / vel);
270-
q_stddev_slip_rate = std::min(q_stddev_slip_rate, ekf_params_.q_stddev_slip_rate_max);
271-
q_stddev_slip_rate = std::max(q_stddev_slip_rate, ekf_params_.q_stddev_slip_rate_min);
269+
// sin(slip) = w*l_r/v
270+
// d(slip)/dt = - w*l_r/v^2 * d(v)/dt + l_r/v * d(w)/dt
271+
q_cov_slip_rate = std::pow(sin_slip * ekf_params_.q_stddev_acc_lat / vel, 2) +
272+
std::pow(q_stddev_yaw_rate * lr_ / vel, 2);
273+
q_cov_slip_rate = std::min(
274+
q_cov_slip_rate, ekf_params_.q_stddev_slip_rate_max * ekf_params_.q_stddev_slip_rate_max);
275+
q_cov_slip_rate = std::max(
276+
q_cov_slip_rate, ekf_params_.q_stddev_slip_rate_min * ekf_params_.q_stddev_slip_rate_min);
272277
}
273278
const float q_cov_x = std::pow(0.5 * ekf_params_.q_stddev_acc_long * dt * dt, 2.0);
274279
const float q_cov_y = std::pow(0.5 * ekf_params_.q_stddev_acc_lat * dt * dt, 2.0);
275280
const float q_cov_yaw = std::pow(q_stddev_yaw_rate * dt, 2.0);
276281
const float q_cov_vel = std::pow(ekf_params_.q_stddev_acc_long * dt, 2.0);
277-
const float q_cov_slip = std::pow(q_stddev_slip_rate * dt, 2.0);
282+
const float q_cov_slip = q_cov_slip_rate * dt * dt;
278283

279284
Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x);
280285
// Rotate the covariance matrix according to the vehicle yaw
@@ -560,20 +565,41 @@ bool NormalVehicleTracker::getTrackedObject(
560565
twist_with_cov.twist.linear.x = X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP));
561566
twist_with_cov.twist.linear.y = X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP));
562567
twist_with_cov.twist.angular.z =
563-
X_t(IDX::VEL) / lr_ * std::sin(X_t(IDX::SLIP)); // yaw_rate = vel_k / l_r * sin(slip_k)
564-
// twist covariance
565-
constexpr double vy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative
568+
X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)) / lr_; // yaw_rate = vel_k * sin(slip_k) / l_r
569+
/* twist covariance
570+
* convert covariance from velocity, slip angle to vx, vy, and yaw angle
571+
*
572+
* vx = vel * cos(slip)
573+
* vy = vel * sin(slip)
574+
* wz = vel * sin(slip) / l_r = vy / l_r
575+
*
576+
*/
577+
566578
constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative
567579
constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative
568580
constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative
569-
twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VEL, IDX::VEL);
570-
twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = vy_cov;
581+
582+
Eigen::MatrixXd cov_jacob(3, 2);
583+
cov_jacob << std::cos(X_t(IDX::SLIP)), -X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)),
584+
std::sin(X_t(IDX::SLIP)), X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)),
585+
std::sin(X_t(IDX::SLIP)) / lr_, X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)) / lr_;
586+
Eigen::MatrixXd cov_twist(2, 2);
587+
cov_twist << P(IDX::VEL, IDX::VEL), P(IDX::VEL, IDX::SLIP), P(IDX::SLIP, IDX::VEL),
588+
P(IDX::SLIP, IDX::SLIP);
589+
Eigen::MatrixXd twist_cov_mat = cov_jacob * cov_twist * cov_jacob.transpose();
590+
591+
twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = twist_cov_mat(0, 0);
592+
twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = twist_cov_mat(0, 1);
593+
twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = twist_cov_mat(0, 2);
594+
twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = twist_cov_mat(1, 0);
595+
twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = twist_cov_mat(1, 1);
596+
twist_with_cov.covariance[utils::MSG_COV_IDX::Y_YAW] = twist_cov_mat(1, 2);
597+
twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = twist_cov_mat(2, 0);
598+
twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_Y] = twist_cov_mat(2, 1);
599+
twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = twist_cov_mat(2, 2);
571600
twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov;
572-
twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = P(IDX::VEL, IDX::SLIP);
573-
twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = P(IDX::SLIP, IDX::VEL);
574601
twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov;
575602
twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov;
576-
twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::SLIP, IDX::SLIP);
577603

578604
// set shape
579605
object.shape.dimensions.x = bounding_box_.length;

0 commit comments

Comments
 (0)