The kalman filter does not take into account the fact that yaw can wrap, this causes a massive jump in the z moment. This is an ugly hack for now that I will fix properly when I get back to lab
Actually Zac on flybook. I am undoing this for now as the wrapping is not flowing through the kalman filter properly and is causing more problems than it solves.
Actually Zac on Flybook,![img_20180704_114007](https://user-images.githubusercontent.com/5616392/42273609-ed8ba5c0-7f89-11e8-8160-9efe902dbdbe.jpg)
The kalman filter does not take into account the fact that yaw can wrap, this causes a massive jump in the z moment. This is an ugly hack for now that I will fix properly when I get back to lab