Automatic calculation of LQR controller that includes yaw angle as a state.

[moorepants]

This hopes to solve two things: 1) drive yaw angle to zero and 2) automatically find the control gains for whatever parameter set and speed we supply.

TODO

• Load parameters from BicycleParameters as the benchmark set and then convert to the Moore values for use in the model (we currently just load the Moore parameters). Another option would be to write a function that converts benchmark parameters to moore parameters.

[moorepants]

If you weight the yaw state more, this actually works (although there is lots more lateral motion than we see in the real experiments):

[moorepants]

@GabrieleDello fyi

[moorepants]

I guess the rider is not only driving heading to zero but also driving lateral position to zero (or near zero). So I could add that feedback loop also.