Add moving average filter
Add throttle/collective manual control i/o scaling
Fix large error values on yaw angle wrap