working kalman and MPU fixes