first try of multithreading using atomic library. Kind of works but still have problems with protection. Next: mutex

added quadratic constraints. Now we notice how the delay affects the system, we dont have time to solve the problem

Fixed some bugs: U_tray was not correctly updated. Added saturation. Change of setpoint correct sign now.

added a lot of matrix functions that linearize/discretize/form the matrices linearized along a trayectory. Need to do next the SxSuSg function

System prepared to start developing MPC controller. Included estimation part and crazyflie_angle_mode part. Need to clean a bit and test it in the real system, but it should work