Skip to content

Commit

Permalink
feat(multi_object_tracker): change vehicle model from constant veloci…
Browse files Browse the repository at this point in the history 
…ty and turn rate model to bicycle model (autowarefoundation#2679)

* try bicycle model

Signed-off-by: yoshiri <[email protected]>

* apply bicycle model to bicycle tracker

Signed-off-by: yoshiri <[email protected]>

Signed-off-by: yoshiri <[email protected]>
  • Loading branch information
@YoshiRi @maxime-clem
YoshiRi authored and maxime-clem committed Jan 30, 2023
1 parent 4c2330d commit c9c21f2
Show file tree
Hide file tree
Showing 6 changed files with 161 additions and 145 deletions.
16 changes: 6 additions & 10 deletions ...ption/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,23 +32,17 @@ class BicycleTracker : public Tracker
private:
KalmanFilter ekf_;
rclcpp::Time last_update_time_;
enum IDX {
X = 0,
Y = 1,
YAW = 2,
VX = 3,
WZ = 4,
};
enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, SLIP = 4 };
struct EkfParams
{
char dim_x = 5;
float q_cov_x;
float q_cov_y;
float q_cov_yaw;
float q_cov_wz;
float q_cov_slip;
float q_cov_vx;
float p0_cov_vx;
float p0_cov_wz;
float p0_cov_slip;
float r_cov_x;
float r_cov_y;
float r_cov_yaw;
Expand All @@ -58,7 +52,9 @@ class BicycleTracker : public Tracker
} ekf_params_;

double max_vx_;
double max_wz_;
double max_slip_;
double lf_;
double lr_;
float z_;

private:
Expand Down
16 changes: 6 additions & 10 deletions ...n/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,23 +32,17 @@ class BigVehicleTracker : public Tracker
private:
KalmanFilter ekf_;
rclcpp::Time last_update_time_;
enum IDX {
X = 0,
Y = 1,
YAW = 2,
VX = 3,
WZ = 4,
};
enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, SLIP = 4 };
struct EkfParams
{
char dim_x = 5;
float q_cov_x;
float q_cov_y;
float q_cov_yaw;
float q_cov_wz;
float q_cov_slip;
float q_cov_vx;
float p0_cov_vx;
float p0_cov_wz;
float p0_cov_slip;
float r_cov_x;
float r_cov_y;
float r_cov_yaw;
Expand All @@ -58,7 +52,9 @@ class BigVehicleTracker : public Tracker
float p0_cov_yaw;
} ekf_params_;
double max_vx_;
double max_wz_;
double max_slip_;
double lf_;
double lr_;
float z_;
double velocity_deviation_threshold_;

Expand Down
16 changes: 6 additions & 10 deletions ...ulti_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,24 +33,18 @@ class NormalVehicleTracker : public Tracker
private:
KalmanFilter ekf_;
rclcpp::Time last_update_time_;
enum IDX {
X = 0,
Y = 1,
YAW = 2,
VX = 3,
WZ = 4,
};
enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, SLIP = 4 };

struct EkfParams
{
char dim_x = 5;
float q_cov_x;
float q_cov_y;
float q_cov_yaw;
float q_cov_wz;
float q_cov_slip;
float q_cov_vx;
float p0_cov_vx;
float p0_cov_wz;
float p0_cov_slip;
float r_cov_x;
float r_cov_y;
float r_cov_yaw;
Expand All @@ -61,7 +55,9 @@ class NormalVehicleTracker : public Tracker
} ekf_params_;

double max_vx_;
double max_wz_;
double max_slip_;
double lf_;
double lr_;
float z_;
double velocity_deviation_threshold_;

Expand Down
85 changes: 47 additions & 38 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 @@ -55,30 +55,30 @@ BicycleTracker::BicycleTracker(
float q_stddev_y = 0.6; // [m/s]
float q_stddev_yaw = tier4_autoware_utils::deg2rad(10); // [rad/s]
float q_stddev_vx = tier4_autoware_utils::kmph2mps(10); // [m/(s*s)]
float q_stddev_wz = tier4_autoware_utils::deg2rad(25); // [rad/(s*s)]
float q_stddev_slip = tier4_autoware_utils::deg2rad(25); // [rad/(s*s)]
float r_stddev_x = 0.6; // [m]
float r_stddev_y = 0.4; // [m]
float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad]
float p0_stddev_x = 0.8; // [m/s]
float p0_stddev_y = 0.5; // [m/s]
float p0_stddev_yaw = tier4_autoware_utils::deg2rad(1000); // [rad/s]
float p0_stddev_vx = tier4_autoware_utils::kmph2mps(1000); // [m/(s*s)]
float p0_stddev_wz = tier4_autoware_utils::deg2rad(10); // [rad/(s*s)]
float p0_stddev_slip = tier4_autoware_utils::deg2rad(10); // [rad/(s*s)]
ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0);
ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0);
ekf_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0);
ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0);
ekf_params_.q_cov_wz = std::pow(q_stddev_wz, 2.0);
ekf_params_.q_cov_slip = std::pow(q_stddev_slip, 2.0);
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);
ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0);
ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0);
ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0);
ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0);
ekf_params_.p0_cov_wz = std::pow(p0_stddev_wz, 2.0);
ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0);
max_vx_ = tier4_autoware_utils::kmph2mps(100); // [m/s]
max_wz_ = tier4_autoware_utils::deg2rad(30); // [rad/s]
max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad/s]

// initialize X matrix
Eigen::MatrixXd X(ekf_params_.dim_x, 1);
Expand All @@ -87,11 +87,10 @@ BicycleTracker::BicycleTracker(
X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation);
if (object.kinematics.has_twist) {
X(IDX::VX) = object.kinematics.twist_with_covariance.twist.linear.x;
X(IDX::WZ) = object.kinematics.twist_with_covariance.twist.angular.z;
} else {
X(IDX::VX) = 0.0;
X(IDX::WZ) = 0.0;
}
X(IDX::SLIP) = 0.0;

// initialize P matrix
Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x);
Expand All @@ -110,7 +109,7 @@ BicycleTracker::BicycleTracker(
P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y);
P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw;
P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx;
P(IDX::WZ, IDX::WZ) = ekf_params_.p0_cov_wz;
P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip;
} else {
P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X];
P(IDX::X, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y];
Expand All @@ -121,12 +120,10 @@ BicycleTracker::BicycleTracker(
if (object.kinematics.has_twist_covariance) {
P(IDX::VX, IDX::VX) =
object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X];
P(IDX::WZ, IDX::WZ) =
object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW];
} else {
P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx;
P(IDX::WZ, IDX::WZ) = ekf_params_.p0_cov_wz;
}
P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip;
}

if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
Expand All @@ -136,6 +133,11 @@ BicycleTracker::BicycleTracker(
bounding_box_ = {1.0, 0.5, 1.7};
}
ekf_.init(X, P);

// Set lf, lr
double point_ratio = 0.5; // comes to front if larger
lf_ = bounding_box_.length * point_ratio;
lr_ = bounding_box_.length * (1.0 - point_ratio);
}

bool BicycleTracker::predict(const rclcpp::Time & time)
Expand All @@ -150,47 +152,53 @@ bool BicycleTracker::predict(const rclcpp::Time & time)

bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const
{
/* == Nonlinear model ==
/* == Nonlinear model == static bicycle model
*
* x_{k+1} = x_k + vx_k * cos(yaw_k) * dt
* y_{k+1} = y_k + vx_k * sin(yaw_k) * dt
* yaw_{k+1} = yaw_k + (wz_k) * dt
* x_{k+1} = x_k + vx_k * cos(yaw_k + slip_k) * dt
* y_{k+1} = y_k + vx_k * sin(yaw_k + slip_k) * dt
* yaw_{k+1} = yaw_k + vx_k / l_r * sin(slip_k) * dt
* vx_{k+1} = vx_k
* wz_{k+1} = wz_k
* slip_{k+1} = slip_k
*
*/

/* == Linearized model ==
*
* A = [ 1, 0, -vx*sin(yaw)*dt, cos(yaw)*dt, 0]
* [ 0, 1, vx*cos(yaw)*dt, sin(yaw)*dt, 0]
* [ 0, 0, 1, 0, dt]
* [ 0, 0, 0, 1, 0]
* [ 0, 0, 0, 0, 1]
* A = [ 1, 0, -vx*sin(yaw+slip)*dt, cos(yaw+slip)*dt, -vx*sin(yaw+slip)*dt]
* [ 0, 1, vx*cos(yaw+slip)*dt, sin(yaw+slip)*dt, vx*cos(yaw+slip)*dt]
* [ 0, 0, 1, 1/l_r*sin(slip)*dt, vx/l_r*cos(slip)*dt]
* [ 0, 0, 0, 1, 0]
* [ 0, 0, 0, 0, 1]
*/

// 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));
const double sin_yaw = std::sin(X_t(IDX::YAW));
const double cos_yaw = std::cos(X_t(IDX::YAW) + X_t(IDX::SLIP));
const double sin_yaw = std::sin(X_t(IDX::YAW) + X_t(IDX::SLIP));
const double cos_slip = std::cos(X_t(IDX::SLIP));
const double sin_slip = std::sin(X_t(IDX::SLIP));
const double vx = X_t(IDX::VX);
const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW));

// X t+1
Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state
X_next_t(IDX::X) = X_t(IDX::X) + X_t(IDX::VX) * cos_yaw * dt; // dx = v * cos(yaw)
X_next_t(IDX::Y) = X_t(IDX::Y) + X_t(IDX::VX) * sin_yaw * dt; // dy = v * sin(yaw)
X_next_t(IDX::YAW) = X_t(IDX::YAW) + (X_t(IDX::WZ)) * dt; // dyaw = omega
Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state
X_next_t(IDX::X) = X_t(IDX::X) + vx * cos_yaw * dt; // dx = v * cos(yaw)
X_next_t(IDX::Y) = X_t(IDX::Y) + vx * sin_yaw * dt; // dy = v * sin(yaw)
X_next_t(IDX::YAW) = X_t(IDX::YAW) + vx / lr_ * sin_slip * dt; // dyaw = omega
X_next_t(IDX::VX) = X_t(IDX::VX);
X_next_t(IDX::WZ) = X_t(IDX::WZ);
X_next_t(IDX::SLIP) = X_t(IDX::SLIP);

// A
Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x);
A(IDX::X, IDX::YAW) = -X_t(IDX::VX) * sin_yaw * dt;
A(IDX::X, IDX::YAW) = -vx * sin_yaw * dt;
A(IDX::X, IDX::VX) = cos_yaw * dt;
A(IDX::Y, IDX::YAW) = X_t(IDX::VX) * cos_yaw * dt;
A(IDX::X, IDX::SLIP) = -vx * sin_yaw * dt;
A(IDX::Y, IDX::YAW) = vx * cos_yaw * dt;
A(IDX::Y, IDX::VX) = sin_yaw * dt;
A(IDX::YAW, IDX::WZ) = dt;
A(IDX::Y, IDX::SLIP) = vx * cos_yaw * dt;
A(IDX::YAW, IDX::VX) = 1.0 / lr_ * sin_slip * dt;
A(IDX::YAW, IDX::SLIP) = vx / lr_ * cos_slip * dt;

// Q
Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x);
Expand All @@ -204,7 +212,7 @@ bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const
Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y);
Q(IDX::YAW, IDX::YAW) = ekf_params_.q_cov_yaw * dt * dt;
Q(IDX::VX, IDX::VX) = ekf_params_.q_cov_vx * dt * dt;
Q(IDX::WZ, IDX::WZ) = ekf_params_.q_cov_wz * dt * dt;
Q(IDX::SLIP, IDX::SLIP) = ekf_params_.q_cov_slip * dt * dt;
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);

Expand Down Expand Up @@ -315,8 +323,8 @@ bool BicycleTracker::measureWithPose(
if (!(-max_vx_ <= X_t(IDX::VX) && X_t(IDX::VX) <= max_vx_)) {
X_t(IDX::VX) = X_t(IDX::VX) < 0 ? -max_vx_ : max_vx_;
}
if (!(-max_wz_ <= X_t(IDX::WZ) && X_t(IDX::WZ) <= max_wz_)) {
X_t(IDX::WZ) = X_t(IDX::WZ) < 0 ? -max_wz_ : max_wz_;
if (!(-max_slip_ <= X_t(IDX::SLIP) && X_t(IDX::SLIP) <= max_slip_)) {
X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -max_slip_ : max_slip_;
}
ekf_.init(X_t, P_t);
}
Expand Down Expand Up @@ -420,7 +428,8 @@ bool BicycleTracker::getTrackedObject(

// twist
twist_with_cov.twist.linear.x = X_t(IDX::VX);
twist_with_cov.twist.angular.z = X_t(IDX::WZ);
twist_with_cov.twist.angular.z =
X_t(IDX::VX) / lr_ * std::sin(X_t(IDX::SLIP)); // yaw_rate = vx_k / l_r * sin(slip_k)
// twist covariance
constexpr double vy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative
constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative
Expand All @@ -429,11 +438,11 @@ bool BicycleTracker::getTrackedObject(
twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VX, IDX::VX);
twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = vy_cov;
twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov;
twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = P(IDX::VX, IDX::WZ);
twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = P(IDX::WZ, IDX::VX);
twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = P(IDX::VX, IDX::SLIP);
twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = P(IDX::SLIP, IDX::VX);
twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov;
twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov;
twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::WZ, IDX::WZ);
twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::SLIP, IDX::SLIP);

// set shape
object.shape.dimensions.x = bounding_box_.length;
Expand Down
Loading

0 comments on commit c9c21f2

Please sign in to comment.