[Feature]: Overhaul the Stanley Controller for Steering-Tuning

[hellschwalex]

Description

The Stanley Controller should calculate good steering-inputs at higher velocities compared to the PurePursuit Controller, that we want to use at lower velocities.

• Search for more literature concerning the correct implementation for stanley algorithm
• Check the current algorithm and its implementation
• Check the current tuning-parameters, correctly used?
• Tune Stanley for higher velocities

Definition of Done

• newly implemented and tuned Stanley overall outperforms old implementation
• newly implemented and tuned Stanley always outperforms current PurePursuit implementation from a break-off velocity on (the higher the velocity, the more Stanly must outperform PP)
• Overall good path-tracking and steering