Add moving average for yaw
Add moving average for yaw

zegervdv/Quadcopter,zegervdv/Quadcopter,zegervdv/STM-Quadcopter,zegervdv/Quadcopter,zegervdv/STM-Quadcopter,zegervdv/STM-Quadcopter,zegervdv/Quadcopter,zegervdv/STM-Quadcopter