Q (state gains) and R (control action gains) are in world frame, causing them to be inaccurate for nonzero psi

[LouKordos]

The state gains Q and the control input gains R are defined in the world frame, which of course does not rotate with the body, meaning that a nonzero yaw angle (psi), or any nonzero angle for that matter, invalidates these gains.

As an example, at psi=90deg, the X position and velocity gains will actually be the Y position and velocity gains and vice versa. As the body behaves differently in each direction, this bug makes the robot unstable when turning in place or walking at an angle, i.e. resulting in the MPC lowering the body gradually until it hits the ground or fails otherwise.

The same happens in the Jupyter notebook applied to the purely mathematical model, which indicates it is an actual bug and not only apparent in a physics-based simulation.

The fix for this is to move the gains Q and R from compile-time parameters to runtime parameters in P, one Q and R combination for every time step in the prediction horizon. These gains can then be updated at every MPC loop iteration based on previously predicted states.

[LouKordos]

This commit introduces functionality for specifying Q and R for every timestep at runtime.

[LouKordos]

As the previously used weights have been assumed to be in body frame during tuning, the MPC always assumed they were in world frame. Thus, the current weights will be renamed to Q_body and Q_world will be determined for the MPC at every time step. More specifically, this formula shows the relationship between Q_body and Q_world:

Here, the Q_w will be calculated with R_z(-psi).T Q_body R_z(-psi), where R_z is the two-dimensional rotation matrix transforming from body frame to world frame, which is inverted by changing sign from psi to -psi. This way, the position (and velocity) error in world frame is transformed into body frame. After rearranging the right hand side, the terms are still equal but the term for Q_world becomes apparent.

Many thanks @WPBack for explaining!

[LouKordos]

As can be seen below, the implementation of this fix successfully keeps Z position at desired instead of lowering. pos_error_gain still needs to be adjusted to give the best results, though.

[LouKordos]

87 fixes this, closing.