Skip to content

Commit dbdc238

Browse files
technolojinpre-commit-ci[bot]YoshiRiHansRobo
authored
feat(multi_object_tracker): mot bicycle model revision (#6082)
* fix: bicycle model length on big vehicle Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: rear axis position fix in all modalities Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: repeat predict to limit dt Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: implement repeating prediction to all modalities Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: bicycle model revision Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: fix centripetal acceleration, jacobian Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * style(pre-commit): autofix Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * chore: equation on description, velocity index fix Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: markdown math Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * chore: refactor params Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * feat: model slip process rate Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * feat: yaw rate limits Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * chore: refactor vx to vel, follow actual definition Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: acceleration uncertainty equation fix Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: twist covariance matrix Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * chore: explanations of Kalman filter matrices Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * feat: slip angle process noise model revised Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * feat: parameter tuning Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: use mahalanobis distance gate with larger threshold Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * chore: align tracker format Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * style(pre-commit): autofix --------- Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Yoshi Ri <yoshiyoshidetteiu@gmail.com> Co-authored-by: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>
1 parent 76cc954 commit dbdc238

File tree

8 files changed

+702
-401
lines changed

8 files changed

+702
-401
lines changed

‎perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp

+10-8
Original file line numberDiff line numberDiff line change
@@ -32,16 +32,18 @@ class BicycleTracker : public Tracker
3232
private:
3333
KalmanFilter ekf_;
3434
rclcpp::Time last_update_time_;
35-
enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, SLIP = 4 };
35+
enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 };
3636
struct EkfParams
3737
{
3838
char dim_x = 5;
39-
float q_cov_x;
40-
float q_cov_y;
41-
float q_cov_yaw;
42-
float q_cov_slip;
43-
float q_cov_vx;
44-
float p0_cov_vx;
39+
float q_stddev_acc_long;
40+
float q_stddev_acc_lat;
41+
float q_stddev_yaw_rate_min;
42+
float q_stddev_yaw_rate_max;
43+
float q_cov_slip_rate_min;
44+
float q_cov_slip_rate_max;
45+
float q_max_slip_angle;
46+
float p0_cov_vel;
4547
float p0_cov_slip;
4648
float r_cov_x;
4749
float r_cov_y;
@@ -51,7 +53,7 @@ class BicycleTracker : public Tracker
5153
float p0_cov_yaw;
5254
} ekf_params_;
5355

54-
double max_vx_;
56+
double max_vel_;
5557
double max_slip_;
5658
double lf_;
5759
double lr_;

‎perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp

+11-9
Original file line numberDiff line numberDiff line change
@@ -32,26 +32,28 @@ class BigVehicleTracker : public Tracker
3232
private:
3333
KalmanFilter ekf_;
3434
rclcpp::Time last_update_time_;
35-
enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, SLIP = 4 };
35+
enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 };
3636
struct EkfParams
3737
{
3838
char dim_x = 5;
39-
float q_cov_x;
40-
float q_cov_y;
41-
float q_cov_yaw;
42-
float q_cov_slip;
43-
float q_cov_vx;
44-
float p0_cov_vx;
39+
float q_stddev_acc_long;
40+
float q_stddev_acc_lat;
41+
float q_stddev_yaw_rate_min;
42+
float q_stddev_yaw_rate_max;
43+
float q_cov_slip_rate_min;
44+
float q_cov_slip_rate_max;
45+
float q_max_slip_angle;
46+
float p0_cov_vel;
4547
float p0_cov_slip;
4648
float r_cov_x;
4749
float r_cov_y;
4850
float r_cov_yaw;
49-
float r_cov_vx;
51+
float r_cov_vel;
5052
float p0_cov_x;
5153
float p0_cov_y;
5254
float p0_cov_yaw;
5355
} ekf_params_;
54-
double max_vx_;
56+
double max_vel_;
5557
double max_slip_;
5658
double lf_;
5759
double lr_;

‎perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp

+12-9
Original file line numberDiff line numberDiff line change
@@ -33,28 +33,30 @@ class NormalVehicleTracker : public Tracker
3333
private:
3434
KalmanFilter ekf_;
3535
rclcpp::Time last_update_time_;
36-
enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, SLIP = 4 };
36+
enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 };
3737

3838
struct EkfParams
3939
{
4040
char dim_x = 5;
41-
float q_cov_x;
42-
float q_cov_y;
43-
float q_cov_yaw;
44-
float q_cov_slip;
45-
float q_cov_vx;
46-
float p0_cov_vx;
41+
float q_stddev_acc_long;
42+
float q_stddev_acc_lat;
43+
float q_stddev_yaw_rate_min;
44+
float q_stddev_yaw_rate_max;
45+
float q_cov_slip_rate_min;
46+
float q_cov_slip_rate_max;
47+
float q_max_slip_angle;
48+
float p0_cov_vel;
4749
float p0_cov_slip;
4850
float r_cov_x;
4951
float r_cov_y;
5052
float r_cov_yaw;
51-
float r_cov_vx;
53+
float r_cov_vel;
5254
float p0_cov_x;
5355
float p0_cov_y;
5456
float p0_cov_yaw;
5557
} ekf_params_;
5658

57-
double max_vx_;
59+
double max_vel_;
5860
double max_slip_;
5961
double lf_;
6062
double lr_;
@@ -88,6 +90,7 @@ class NormalVehicleTracker : public Tracker
8890
const rclcpp::Time & time,
8991
autoware_auto_perception_msgs::msg::TrackedObject & object) const override;
9092
void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform);
93+
double getMeasurementYaw(const autoware_auto_perception_msgs::msg::DetectedObject & object);
9194
virtual ~NormalVehicleTracker() {}
9295
};
9396

‎perception/multi_object_tracker/models.md

+27-18
Original file line numberDiff line numberDiff line change
@@ -11,21 +11,21 @@ CTRV model is a model that assumes constant turn rate and velocity magnitude.
1111
- state transition equation
1212

1313
$$
14-
\begin{align*}
15-
x_{k+1} &= x_k + v_{x_k} \cos(\psi_k) \cdot dt \\
16-
y_{k+1} &= y_k + v_{x_k} \sin(\psi_k) \cdot dt \\
17-
\psi_{k+1} &= \psi_k + \dot{\psi}_k \cdot dt \\
18-
v_{x_{k+1}} &= v_{x_k} \\
19-
\dot{\psi}_{k+1} &= \dot{\psi}_k \\
20-
\end{align*}
14+
\begin{aligned}
15+
x_{k+1} & = x_{k} + v_{k} \cos(\psi_k) \cdot {d t} \\
16+
y_{k+1} & = y_{k} + v_{k} \sin(\psi_k) \cdot {d t} \\
17+
\psi_{k+1} & = \psi_k + \dot\psi_{k} \cdot {d t} \\
18+
v_{k+1} & = v_{k} \\
19+
\dot\psi_{k+1} & = \dot\psi_{k} \\
20+
\end{aligned}
2121
$$
2222

2323
- jacobian
2424

2525
$$
2626
A = \begin{bmatrix}
27-
1 & 0 & -v_x \sin(\psi) \cdot dt & \cos(\psi) \cdot dt & 0 \\
28-
0 & 1 & v_x \cos(\psi) \cdot dt & \sin(\psi) \cdot dt & 0 \\
27+
1 & 0 & -v \sin(\psi) \cdot dt & \cos(\psi) \cdot dt & 0 \\
28+
0 & 1 & v \cos(\psi) \cdot dt & \sin(\psi) \cdot dt & 0 \\
2929
0 & 0 & 1 & 0 & dt \\
3030
0 & 0 & 0 & 1 & 0 \\
3131
0 & 0 & 0 & 0 & 1 \\
@@ -40,17 +40,20 @@ The merit of using this model is that it can prevent unintended yaw rotation whe
4040
![kinematic_bicycle_model](image/kinematic_bicycle_model.png)
4141

4242
- **state variable**
43-
- pose( $x,y$ ), velocity( $v$ ), yaw( $\psi$ ), and slip angle ( $\beta$ )
44-
- $[x_{k} ,y_{k} , v_{k} , \psi_{k} , \beta_{k} ]^\mathrm{T}$
43+
- pose( $x,y$ ), yaw( $\psi$ ), velocity( $v$ ), and slip angle ( $\beta$ )
44+
- $[x_{k}, y_{k}, \psi_{k}, v_{k}, \beta_{k} ]^\mathrm{T}$
4545
- **Prediction Equation**
4646
- $dt$: sampling time
47+
- $w_{k} = \dot\psi_{k} = \frac{ v_{k} \sin \left( \beta_{k} \right) }{ l_r }$ : angular velocity
4748

4849
$$
4950
\begin{aligned}
50-
x_{k+1} & =x_{k}+v_{k} \cos \left(\psi_{k}+\beta_{k}\right) d t \\
51-
y_{k+1} & =y_{k}+v_{k} \sin \left(\psi_{k}+\beta_{k}\right) d t \\
52-
v_{k+1} & =v_{k} \\
53-
\psi_{k+1} & =\psi_{k}+\frac{v_{k}}{l_{r}} \sin \beta_{k} d t \\
51+
x_{k+1} & = x_{k} + v_{k} \cos \left( \psi_{k}+\beta_{k} \right) {d t}
52+
-\frac{1}{2} \left\lbrace w_k v_k \sin \left(\psi_{k}+\beta_{k} \right) \right\rbrace {d t}^2\\
53+
y_{k+1} & = y_{k} + v_{k} \sin \left( \psi_{k}+\beta_{k} \right) {d t}
54+
+\frac{1}{2} \left\lbrace w_k v_k \cos \left(\psi_{k}+\beta_{k} \right) \right\rbrace {d t}^2\\
55+
\psi_{k+1} & =\psi_{k} + w_k {d t} \\
56+
v_{k+1} & = v_{k} \\
5457
\beta_{k+1} & =\beta_{k}
5558
\end{aligned}
5659
$$
@@ -59,9 +62,15 @@ $$
5962

6063
$$
6164
\frac{\partial f}{\partial \mathrm x}=\left[\begin{array}{ccccc}
62-
1 & 0 & -v \sin (\psi+\beta) d t & v \cos (\psi+\beta) & -v \sin (\psi+\beta) d t \\
63-
0 & 1 & v \cos (\psi+\beta) d t & v \sin (\psi+\beta) & v \cos (\psi+\beta) d t \\
64-
0 & 0 & 1 & \frac{1}{l_r} \sin \beta d t & \frac{v}{l_r} \cos \beta d t \\
65+
1 & 0
66+
& v \cos (\psi+\beta) {d t} - \frac{1}{2} \left\lbrace w v \cos \left( \psi+\beta \right) \right\rbrace {d t}^2
67+
& \sin (\psi+\beta) {d t} - \left\lbrace w \sin \left( \psi+\beta \right) \right\rbrace {d t}^2
68+
& -v \sin (\psi+\beta) {d t} - \frac{v^2}{2l_r} \left\lbrace \cos(\beta)\sin(\psi+\beta)+\sin(\beta)\cos(\psi+\beta) \right\rbrace {d t}^2 \\
69+
0 & 1
70+
& v \sin (\psi+\beta) {d t} - \frac{1}{2} \left\lbrace w v \sin \left( \psi+\beta \right) \right\rbrace {d t}^2
71+
& \cos (\psi+\beta) {d t} + \left\lbrace w \cos \left( \psi+\beta \right) \right\rbrace {d t}^2
72+
& v \cos (\psi+\beta) {d t} + \frac{v^2}{2l_r} \left\lbrace \cos(\beta)\cos(\psi+\beta)-\sin(\beta)\sin(\psi+\beta) \right\rbrace {d t}^2 \\
73+
0 & 0 & 1 & \frac{1}{l_r} \sin \beta {d t} & \frac{v}{l_r} \cos \beta d t \\
6574
0 & 0 & 0 & 1 & 0 \\
6675
0 & 0 & 0 & 0 & 1
6776
\end{array}\right]

‎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 = false;
204+
if (3.035 /*99%*/ <= mahalanobis_dist) passed_gate = false;
205205
}
206206
// 2d iou gate
207207
if (passed_gate) {

0 commit comments

Comments
 (0)