Kalman filter works with calibration removed. Zero angle has been hardcoded to be 0.