Steer Rate consideration for Lateral Control

[Gernby]

This fix adds steering rate consideration to latcontrol.py, to reduce error (noise) in the torque calculation.

This has the following benefits:

• Steering response is improved across all vehicle speed ranges
• Latency is reduced
• Overshooting / Undershooting is virtually eliminated
• Actual angle follows desired angle much more closely
• Dependency and sensitivity to PID "tuning" is reduced
• Feed forward error due to suspension alignment, wind, and slope is reduced
• Sample error from low resolution sensor data is reduced
• Increases fluid movement of the steering mechanisms between planner updates
• May allow lower steering rate cost and / or actuator delays
• May increase peak achievable steering angles on some models, due to smoother sweep

[Gernby]

Here is a video from a test with very low values for steerRateCost and steerActuatorDelay.
https://www.youtube.com/watch?v=whMI91o0TOU

[Gernby]

After feedback from several testers (Honda's and Toyota's), it was clear that the previous logic was better in cases that the steer rate was low OR the steer error was low.

My last commit seems like the best of both.

[Gernby]

This PR has become rather outdated, since I've continued making progress using various strategies. However, I finally made a large break-through yesterday, which results in virtually "ideal" steering response.

It needs testing on other vehicles, and the code needs to be refactored (hopefully with your input), so I'm not sure whether this PR should be closed, or not. Please advise.

Also, here is a video of the results, which also explains how the steering system can be treated as a resonant circuit by making the PID inputs more reactive, inductive, and resistive.
https://www.youtube.com/watch?v=8sTyDbXJIRo

This shows how quickly and smoothly the steering recovers after manual disturbance of lane position.

[Gernby]

As I said, it's not "PR ready" yet, but you can view the logic in this branch.
https://github.com/Gernby/openpilot/tree/Feed-Forward-Accel-Rate

[Gernby]

I'm closing this PR since the enhancements have been ongoing. I will create a new PR (or multiple separate PR's) for the following soon:

• Add interpolation of desired angle between MPC updates
• Convert torque logic to a virtual "resonant circuit"
• Filter latent steering error from MPC's input to reduce feedback noise
• Split Feed-Forward logic into 2 modes (rate or angle)
• Smooth steering by limiting angular acceleration
• Improve accuracy of the future state using observed steering rate and acceleration

These enhancements exist in the branch below, and have been tested with success on Honda's, Toyota's, and Tesla's.

https://github.com/Gernby/openpilot/tree/Feed-Forward-Resonant