Kalman Filter

X,Y angle seem to work independently. Z is off by ~20 deg. 

The angles skew after a while, but we could change the parameters.