Implement Stanley Pursuit for rovers

[slgrobotics]

Solved Problem

PX4 library offers L1 and Pure Pursuit library modules, which can be used for vehicles. Another common trajectory following logic is Stanley Pursuit, which is currently missing from the library.

It is briefly described here:

https://dingyan89.medium.com/three-methods-of-vehicle-lateral-control-pure-pursuit-stanley-and-mpc-db8cc1d32081

Solution

This PR adds a basic implementation of Stanley Pursuit

Changelog Entry

For release notes:

New parameters: ST_XTRACK_GAIN, ST_SOFTENING

Test coverage

• Unit tests included in the PR

Note

In my experience so far, Stanley Pursuit is the best fit for my gas-driven zero-turn lawnmower, compared to L1 and Pure Pursuit, which I also tried. When close line following is a priority, Stanley algorithm provides minimal oscillations and directly targets to minimize cross-track distance.