Step 3: AP_Math: Update shaping to use improved jerk limited algorithm.

[lthall]

The current real time S-Curves have two problems that this PR addresses:

1. There is a small overshoot as it approaches the target position in some combinations of input.
2. The deceleration it generates is significantly less than the limits specify.

This PR produces the full deceleration specified by the square root controller and maintains the time constant based approach to the position target. This time constant based approach makes this algorithm act as a filter making it suitable for noisy or inputs and means that the algorithm does not oscillate or buzz near the set-point.

Below is a plot showing the original square root controller, the current Real Time S Curve, This Jerk Square Root algorithm and the standard rectangular jerk S Curve (Optimal).

There are two reasons we would choose not to use the Optimal solution:

1. Computational Complexity.
2. Lack of any soft approach to set-point.

Both of these make Optimal less appealing to high update rate applications.

Meant to be merged after #31870 and #31848

[Copilot]

Copilot encountered an error and was unable to review this pull request. You can try again by re-requesting a review.

[Copilot]

Pull request overview

Copilot reviewed 2 out of 2 changed files in this pull request and generated 2 comments.

---

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

[amilcarlucas]

The mem cost seams reasonably small:

Binary Name      Text [B]        Data [B]     BSS (B)        Total Flash Change [B] (%)      Flash Free After PR (B)
---------------  --------------  -----------  -------------  ----------------------------  -------------------------
ardurover        280 (+0.0197%)  0 (0.0000%)  0 (0.0000%)    280 (+0.0196%)                                   277564
arducopter-heli  780 (+0.0500%)  0 (0.0000%)  4 (+0.0016%)   780 (+0.0498%)                                   137276
arducopter       780 (+0.0499%)  0 (0.0000%)  4 (+0.0016%)   780 (+0.0497%)                                   134340
arduplane        780 (+0.0501%)  0 (0.0000%)  -4 (-0.0016%)  780 (+0.0499%)                                   140680
antennatracker   0 (0.0000%)     0 (0.0000%)  0 (0.0000%)    0 (0.0000%)                                      565104
blimp            0 (0.0000%)     0 (0.0000%)  0 (0.0000%)    0 (0.0000%)                                      545748
ardusub          652 (+0.0463%)  0 (0.0000%)  4 (+0.0015%)   652 (+0.0462%)                                   292140

[andyp1per]

I get the intent, but I don't understand what this affects as it seems to change the behaviour of existing methods?

[lthall]

I get the intent, but I don't understand what this affects as it seems to change the behaviour of existing methods?

Yes, it performs the same behaviour as the existing method but has improved stopping behaviour.

1. It stops with a jerk / acceleration profile that is closer to ideal.
2. It does not overshoot in some cases.

I don't think it is inferior in any way but offers significant improvements.

[Copilot]

Copilot encountered an error and was unable to review this pull request. You can try again by re-requesting a review.

[Ferruccio1984]

Exstensively tested on SITL. Remarkable improvement of acceleration and deceleration shaping with overall improvement of the speed profile.
A Python script has been used for testing the behavior in Guided flight mode ensuring consistency across tests.
The following plots offer a comparison of the results :

Sharp corners detail:

Speed profile:

Thanks Leonard!

[IamPete1]

Looks good, Thanks

[Hwurzburg]

I reverted this change to my POI autotest...I don't know how it could have passed without the delay to ARRIVE at the new position...it certainly does not if I run the test locally...the wait times out while it is still traveling to its destination...it takes over 15sec just to get there, and it must be that the mount check is loosened enough that it passes while its far enough away and low enough to pass.....