fix(mpc): bilinear discretization

[TakaHoribe]

Description

The bilinear discretization used in the MPC model needs to be fixed.

Related links

Tests performed

To see the accuracy of the model, steering rate information is visualized below.
The important lines are:

• steering rate estimated in the model (red on the left plot)
• steering rate measured by the sensor (green on the left plot)

These two plots should fit in the perfect model. (Although there is a time shift due to the delay compensation.) You can see that the current model is not good, and the bilinear one has the best fit.

Current Implementation
(Ad is bilinear, but Bd, Cd, Wd is Euler)

Euler

Bilinear (fixed)

Note: test site in simulation (kashiwanoha)

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.

[TakaHoribe]

@boyali Would you please check this fix for the bilinear discretization?

[codecov]

Codecov Report

Base: 10.85% // Head: 10.85% // No change to project coverage 👍

Coverage data is based on head (15d0e83) compared to base (15d0e83).
Patch has no changes to coverable lines.

❗ Current head 15d0e83 differs from pull request most recent head f35af14. Consider uploading reports for the commit f35af14 to get more accurate results

Additional details and impacted files

@@           Coverage Diff           @@
##             main    #1567   +/-   ##
=======================================
  Coverage   10.85%   10.85%           
=======================================
  Files        1176     1176           
  Lines       77652    77652           
  Branches    19386    19386           
=======================================
  Hits         8432     8432           
  Misses      59978    59978           
  Partials     9242     9242           

Flag	Coverage Δ
total	10.85% <0.00%> (ø)

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.

[boyali]

I don't know if you are using d_d. Even D = 0 in the continuous state space is not zero in the discrete version.

y(k) = Cx(k) + Du(k)

Here my implementation in the Autoware Control Toolbox.

auto const &&mat1_ATs = I - A_ * Ts / 2.;
auto const &&inv1_ATs = mat1_ATs.inverse();

Ad_ = inv1_ATs * (I + A_ * Ts / 2.);
Bd_ = inv1_ATs * B_ * Ts;
Cd_ = C_ * inv1_ATs;
Dd_ = D_ + C_ * Bd_ / 2.;

Dd = D + C inv(1 - Adt/2) Bdt/2

[TakaHoribe]

Thank you for the comment. I misunderstood. Since we can assume the system has a ZOH input, we don't need to convert the measurement equation.

[tkimura4]

We tested it on a real vehicle and found no problems.