Add LQR controller to C

Description of the problem

Need to port LQR controller from the Simulink simulation to the C code running on the quad.

Proposed solution

I plan to add matrix constant and matrix multiplication blocks to the control graph and then implement a graph that represents the LQR controller.