Remove debug code
More of that "The Matrix" stuff
Use coupled axes in Kalman filter