Improve the complememtry filter (Fir Gyro and Accel fusion usage)