MPC controller + E2E simulation in MuJoCo

[bbworld1]

This pull request adds a cvxpygen-based MPC controller (using the osqp) solver. It also adds a Mujoco simulation environment which can be used to test ASTRAFlightSoftware end-to-end.

[quackduck]

Does this controller hard-code a "trajectory"?

[bbworld1]

No, this specific controller targets a specific position to stabilize around. Trajectory tracking probably should have a slightly different formulation

[quackduck]

This is a big PR and a lot of work! I probably need to be walked through a bunch of this so I understand how it works. Some questions I have are:

1. Does this work fine with the Simulink framework? How much effort would it be to run this controller within Simulink using the C caller stuff we've been trying to do?
2. Can it follow a supplied trajectory? If I understand correctly, the controller is currently hard-coded to do a landing at a certain point?
3. It seems like part of the MEKFEstimator now directly return sensor data instead of doing the estimation? Am I reading it wrong?
4. (Minor thing about organization - can we use git submodules to make it clear what parts of the code the team actually owns and what's external library / tinyMPC / etc stuff)

[bbworld1]

This is a big PR and a lot of work! I probably need to be walked through a bunch of this so I understand how it works. Some questions I have are:

1. Does this work fine with the Simulink framework? How much effort would it be to run this controller within Simulink using the C caller stuff we've been trying to do?
2. Can it follow a supplied trajectory? If I understand correctly, the controller is currently hard-coded to do a landing at a certain point?
3. It seems like part of the MEKFEstimator now directly return sensor data instead of doing the estimation? Am I reading it wrong?
4. (Minor thing about organization - can we use git submodules to make it clear what parts of the code the team actually owns and what's external library / tinyMPC / etc stuff)

1. This works fine with the Simulink framework - the flight module architecture means you can run the controller separately of any MuJoCo stuff (swap out MujocoMotor with simulink C caller code)
2. Haven't tested this unfortunately - I think you'd want a slightly more advanced formulation (currently this controller is capable of tracking position, velocity, angle + angular velocity targets, we'd probably want acceleration tracking). In theory it can do it but I'm not sure, ask someone who is good at controls/nav
3. The MEKFEstimator has always directly returned the angular velocity from the gyroscope, since the MEKF implementation we currently have only does angle. I'm hoping someone can actually improve this. Also it directly returns position/velocity estimates from the mujoco sim GPS module, which again i hope someone can improve
4. Sure