瞟了一眼你的程序，互补滤波部分
#define Ki 0.03f &……
你说你2ms采样一次？加上你20深度的滤波，延迟看来会有点大。加大采样率试试吧。互补和卡尔曼都能稳飞的，这个不用怀疑。有时候程序里最简答的地方也会出错，好好看一下吧
……
你对加速度做滤波了吗？最简单的做一下滑动均值滤波，效果可能会好点……
YAW是由陀螺仪积分来的。一是陀螺仪本身有零偏，二是离散的积分本来就存在误差，时间长了自然就就表现出“飘”的状态。这对短时的飞行问题不大，当你觉得四轴yaw飘得较多的时候，打一下摇……
=。= 最近很忙。你要不把工程放上来，有时间的时候给你看看。。。
……
