求教互补滤波

杯具的一天

我的一阶和二阶互补滤波公式是在网上找的 但是滤波后出来的角度不对 不知道是我公式用错了还是变量问题 求救 或者加我QQ讨论514495446    这是一阶，realVals[5]是陀螺仪角速度，fRoll是加速度计求出来的角度   float k=0.075;   float a=k/(k+dt);   float fNewRoll=a*(fNewRoll+realVals[5])+(1-a)*fRoll; 最后滤波后的角度完全和角速度计的角度对不上 这是二阶，   float x1=(fRoll-fNewRoll)*k*k;   //fRoll：根据加速度计算出来的角度   float y1=y1+x1*dt;   float x2=y1+2*k*(fRoll-fNewRoll)+realVals[5];   fNewRoll=fNewRoll+x2*dt;

杯具的一天

搞定了，我的fNewRoll声明放LOOP里面了，搬出去就好了，，，，囧