feat(multi_object_tracker): change vehicle model from constant velocity and turn rate model to bicycle model

[YoshiRi]

Description

Applied bicycle model to avoid unnecessarily yaw rotation when the estimated object speed is small.

• state variable
  • pose( $x,y$ ), velocity( $v$ ), yaw( $\psi$ ), and slip angle ( $\beta$ )
  • $[x_{k} ,y_{k} , v_{k} , \psi_{k} , \beta_{k} ]^\mathrm{T}$
• Prediction Equation
  • $dt$: sampling time

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

• Jacobian

$$ \frac{\partial f}{\partial \mathrm x}=\left[\begin{array}{ccccc} 1 & 0 & -v \sin (\varphi+\beta) d t & v \cos (\varphi+\beta) & -v \sin (\varphi+\beta) d t \\ 0 & 1 & v \cos (\varphi+\beta) d t & v \sin (\varphi+\beta) & v \cos (\varphi+\beta) d t \\ 0 & 0 & 1 & \frac{1}{\operatorname{lr}} \sin \beta d t & \frac{v}{\operatorname{lr}} \cos \beta d t \\ 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 1 \end{array}\right] $$

• Target objects
  • bicycle, normal_vehicle, large_vehicle

Related links

• Paper Reference: Kinematic and dynamic vehicle models for autonomous driving control design
• TIER IV INTERNAL LINK

Tests performed

• Validated with TIER IV Internal rosbag data
• Validated with python simulator code

Before

mock_orig_bike-2023-01-18_14.13.47.mp4

After

mock_fix_bike-2023-01-18_17.10.15.mp4

Notes for reviewers

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]

We still have the following TODOs regarding this feature, but I thought that it is better to improve the tracking behavior as soon as possible, so I have created a PR for the time being.

• Refactoring source code (models should be defined in another file)
• Parameter tuning

[codecov]

Codecov Report

Base: 11.39% // Head: 11.35% // Decreases project coverage by -0.05% ⚠️

Coverage data is based on head (893f056) compared to base (33ecdb5).
Patch coverage: 0.00% of modified lines in pull request are covered.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2679      +/-   ##
==========================================
- Coverage   11.39%   11.35%   -0.05%     
==========================================
  Files        1279     1279              
  Lines       89170    89491     +321     
  Branches    23587    23587              
==========================================
  Hits        10161    10161              
- Misses      68239    68560     +321     
  Partials    10770    10770              

Flag	Coverage Δ		*Carryforward flag
differential	0.00% <0.00%> (?)
total	11.39% <0.00%> (ø)		Carriedforward from 33ecdb5

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

Impacted Files	Coverage Δ
...i_object_tracker/tracker/model/bicycle_tracker.hpp	0.00% <0.00%> (ø)
...ject_tracker/tracker/model/big_vehicle_tracker.hpp	0.00% <0.00%> (ø)
...t_tracker/tracker/model/normal_vehicle_tracker.hpp	0.00% <0.00%> (ø)
...ject_tracker/src/tracker/model/bicycle_tracker.cpp	0.00% <0.00%> (ø)
..._tracker/src/tracker/model/big_vehicle_tracker.cpp	0.00% <0.00%> (ø)
...acker/src/tracker/model/normal_vehicle_tracker.cpp	0.00% <0.00%> (ø)
... and 2 more

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

☔ View full report at Codecov.
📢 Do you have feedback about the report comment? Let us know in this issue.

[yukkysaito]

LGTM