feat(multi_object_tracker): mot bicycle model revision

[technolojin]

Description

Revise bicycle motion model.

1. Bicycle motion model and its Jacobian matrix *VehicleTracke::*VehicleTracker
   • (Current PR) Additionally considering centripetal acceleration
2. Rear and front wheel positions lf_, lr_
   • The wheel position lr is the distance from the center of the object.
   • (Previous) The initial configuration was 80% of its length, which is outside of the object. The wheel position was not updated when the object size is updated.
   • (Current PR)
     • The front wheel position is 30% of the object length, front from the center.
     • The rear wheel position is 25-30% of the object length (depends on object type), rear from the center.
     • The wheel positions will be updated when the object length is updated.
3. Prediction on large delta t *Tracker::predict(const rclcpp::Time & time)
   • The prediction is done by linearized motion estimation.
   • (Previous) While the time interval is too large, the linear assumption induces a large error.
   • (Current PR) If the time interval is large, repeat the Kalman update in divided time steps.
4. Process noise model Eigen::MatrixXd Q
   • (Previous) The process noise was a set of fixed values.
   • (Current PR) The process noise will consider current states and physical limitations.
     • In a high velocity, the object cannot have a high turn-rate, limited by latitudinal acceleration limit.
     • In a very slow velocity, the object cannot turn, limited by the bicycle motion model and its maximum steer angle (slip angle).
     • By modifying the process noise, the following improvements are expected.
     • Yaw stability on straight moving objects (lower Kalman gain)
     • Yaw trackability on turning objects (higher Kalman gain)
5. Tracked object output - twist covariance object.kinematics.twist_with_covariance
   • The tracked object interface has a velocity vector (vx and vy) while the tracker has velocity and slip angle. The covariance matrix is converted based on those relationships.
6. Refactoring: vx to vel (velocity in the tracking model is not a vector.)

This PR contains multiple changes since one of the changes can cause instability of tracking results. The multiple changes are incorporated for stability and trackability.

Related links

https://tier4.atlassian.net/browse/RT1-4914

Tests performed

Simulations of sensing and perception modules were performed under a tier4 deployment.

Blue objects: Detected objects
Red objects: Recorded predicted objects
Yellow objects: Predicted objects done by the revised tracker

• Turning objects at a cross section

mot-bicycle-revision-cross-section5.mp4

The revised tracker tracks yaw rate better than before. As result, the predicted objects follows turning objects well.

mot-bicycle-revision-after-corner.mp4

In tracking objects that turn and then move straight, the uncertainty of the yaw rate is set larger to avoid overestimating the turning.

• Straight cruising object

mot-bicycle-revision-straight-truck.mp4

No performance degradation is observed in a tracking straight-moving object.
It can be seen that the revised tracker reduces position uncertainty (covariance matrix) by accumulating measurements.

Notes for reviewers

The Mahalanobis distances gate threshold is enlarged.

Interface changes

Not applicable.

Effects on system behavior

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

• I've confirmed the contribution guidelines.
• The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

• The PR follows the pull request guidelines.
• The PR has been properly tested.
• The PR has been reviewed by the code owners.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

• There are no open discussions or they are tracked via tickets.
• The PR is ready for merge.

After all checkboxes are checked, anyone who has write access can merge the PR.

[YoshiRi]

LGTM. (There are some trivial suggestions and questions.)

[YoshiRi]

Prediction Equation Changes:

• before

$$ \begin{aligned} x_{k+1} & =x_{k}+v_{k} \cos \left(\psi_{k}+\beta_{k}\right) d t \\ y_{k+1} & =y_{k}+v_{k} \sin \left(\psi_{k}+\beta_{k}\right) d t \\ v_{k+1} & =v_{k} \\ \psi_{k+1} & =\psi_{k}+\frac{v_{k}}{l_{r}} \sin \beta_{k} d t \\ \beta_{k+1} & =\beta_{k} \end{aligned} $$

• after

$$ \begin{aligned} x_{k+1} & = x_{k} + v_{k} \cos \left( \psi_{k}+\beta_{k} \right) {d t} -\frac{1}{2} \left\lbrace w_k v_k \sin \left(\psi_{k}+\beta_{k} \right) \right\rbrace {d t}^2\\ y_{k+1} & = y_{k} + v_{k} \sin \left( \psi_{k}+\beta_{k} \right) {d t} +\frac{1}{2} \left\lbrace w_k v_k \cos \left(\psi_{k}+\beta_{k} \right) \right\rbrace {d t}^2\\ \psi_{k+1} & =\psi_{k} + w_k {d t} \\ v_{k+1} & = v_{k} \\ \beta_{k+1} & =\beta_{k} \end{aligned} $$

[HansRobo]

I updated this branch to import #6278.
This will help you pass build-and-test-differential faster.

[codecov]

Codecov Report

Attention: 331 lines in your changes are missing coverage. Please review.

Comparison is base (742f44a) 14.38% compared to head (7ec0f35) 14.37%.
Report is 1 commits behind head on main.

Files	Patch %	Lines
...acker/src/tracker/model/normal_vehicle_tracker.cpp	0.00%	120 Missing ⚠️
...ject_tracker/src/tracker/model/bicycle_tracker.cpp	0.00%	105 Missing ⚠️
..._tracker/src/tracker/model/big_vehicle_tracker.cpp	0.00%	105 Missing ⚠️
..._tracker/src/data_association/data_association.cpp	0.00%	1 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #6082      +/-   ##
==========================================
- Coverage   14.38%   14.37%   -0.02%     
==========================================
  Files        1907     1907              
  Lines      129935   130076     +141     
  Branches    37616    37616              
==========================================
  Hits        18697    18697              
- Misses      90229    90370     +141     
  Partials    21009    21009              

Flag	Coverage Δ		*Carryforward flag
differential	0.00% <0.00%> (?)
total	14.41% <ø> (+0.02%)	⬆️	Carriedforward from 742f44a

*This pull request uses carry forward flags. Click here to find out more.

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

[yukkysaito]

LGTM