fix(multi_object_tracker): mot multi-step prediction is not work as intended

[technolojin]

Description

The extended kalman filter update process was done in multiple-step when the given time-step is too large.
However, the implementation was not work as intended.

• multi-step prediction

// if dt is too large, shorten dt and repeat prediction
const uint32_t repeat = std::ceil(dt / dt_max_);
const double dt_ = dt / repeat;
for (uint32_t i = 0; i < repeat; ++i) {
  if (!predictStateStep(dt_, tmp_ekf_for_no_update)) {
    return false;
  }
}

The motion model prediction is linealization of non-linear motion model. The prediction step assumes the time difference is small enough. By keeping the prediction time step small, a maximum time step was set and the prediction is to be conducted in multiple times.

• Found symptom: the multi-step prediction is not working as intended.
  Until the reason is found, bring back to the one-step prediction.

FOUND BUG and FIX

A bug was found on predictStateStep methods.

• intention: predict the future state of the variable Kalman filter ekf
• bug: using the member Kalman filter ekf_
• fix: get the state vector from the variable Kalman filter ekf

Tests performed

Tested on recorded data replay recomputation.

Effects on system behavior

Not applicable.

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.

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.

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

[YoshiRi]

@technolojin
Successfully built on my laptop and it seems to work, but could you describe more detail so that we can check what was wrong with past PR and how do you think fixed it?

[codecov]

Codecov Report

Attention: Patch coverage is 0% with 16 lines in your changes are missing coverage. Please review.

Project coverage is 14.78%. Comparing base (fcebec6) to head (c753924).
Report is 10 commits behind head on main.

Files	Patch %	Lines
...ker/src/tracker/motion_model/motion_model_base.cpp	0.00%	12 Missing ⚠️
...i_object_tracker/src/multi_object_tracker_core.cpp	0.00%	1 Missing ⚠️
.../src/tracker/motion_model/bicycle_motion_model.cpp	0.00%	1 Missing ⚠️
...ker/src/tracker/motion_model/ctrv_motion_model.cpp	0.00%	1 Missing ⚠️
...acker/src/tracker/motion_model/cv_motion_model.cpp	0.00%	1 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #6611      +/-   ##
==========================================
- Coverage   14.79%   14.78%   -0.01%     
==========================================
  Files        1920     1920              
  Lines      132349   132430      +81     
  Branches    39339    39339              
==========================================
  Hits        19578    19578              
- Misses      90935    91016      +81     
  Partials    21836    21836              

Flag	Coverage Δ		*Carryforward flag
differential	0.00% <0.00%> (?)
total	14.79% <0.00%> (+<0.01%)	⬆️	Carriedforward from d28afea

*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.

[technolojin]

@YoshiRi
I described definition of multi-step prediction and current issue.
The reason of the issue is under investigation. Until then, I would like to roll-back the part to the previous status, and this PR is for that.

[technolojin]

The bug solution is found. Please wait for the fix.

[technolojin]

the bug is found, the solution is applied, and the explanation is added to the description.

[yukkysaito]

LGTM

[YoshiRi]

LGTM.

The delay compensation worked with my rosbag. (blue: detection, yellow: prediction/tracking)