Terminal Output and hard coded inverse
Hack ready for first test
Test with Hard Code Inverse
Inverse fixed
Test working stage
Different Mavlink Msg for IMU and several EKF Bug fixes - Test Version