关于互补滤波原理

  先介绍一下互补滤波的基本概念，这是阿莫论坛上一个会员的总结：对mpu6050来说，加速度计对四轴或小车的加速度比较敏感，取瞬时值计算倾角误差比较大；而陀螺仪积分得到的角度不受小车加速度的影响，但是随着时间的增加积分漂移和温度漂移带来的误差比较大。所以这两个传感器正好可以弥补相互的缺点。不过要怎么弥补呢？经过上面的介绍是否感觉到可以用滤波器做文章呢？这里讲的互补滤波就是在短时间内采用陀螺仪得到的角度做为最优，定时对加速度采样来的角度进行取平均值来校正陀螺仪的得到的角度。就是，短时间内用陀螺仪比较准确，以它为主；长时间用加速度计比较准确，这时候加大它的比重，这就是互补了，不过滤波在哪里？加速度计要滤掉高频信号，陀螺仪要滤掉低频信号，互补滤波器就是根据传感器特性不同，通过不同的滤波器（高通或低通，互补的），然后再相加得到整个频带的信号，例如，加速度计测倾角，其动态响应较慢，在高频时信号不可用，所以可通过低通抑制高频；陀螺响应快，积分后可测倾角，不过由于零漂等，在低频段信号不好。通过高通滤波可抑制低频噪声。将两者结合，就将陀螺和加表的优点融合起来，得到在高频和低频都较好的信号，互补滤波需要选择切换的频率点，即高通和低通的频率。

 鉴于加速度计低频特性比较好，因为加速度的角度可以直接算出来，没有累积误差，所以长时间后也比较准。而陀螺仪长时间后由于积分误差的累加，会造成输出误差比较大，甚至无法使用。所以用互补滤波法根据他们的特性取长补短进行姿态解算，每过一段时间就让加速度计去校准一下陀螺仪。互补滤波就是在短时间内采用陀螺仪得到的角度做为最优值，定时对加速度采样来的加速度值进行取平均值来校正陀螺仪的得到的角度。短时间内用陀螺仪比较准确，以它为主；长时间用加速度计比较准确，这时候加大它的比重，这就是互补了，不过加速度计要滤掉高频信号，陀螺仪要滤掉低频信号，互补滤波器就是根据传感器特性不同，通过不同的滤波器（高通或低通，互补的），然后再相加得到整个频带的信号。互补是给他们不同的权重加权求和。

互补滤波是这样的：陀螺积分角度+=角速度*dt；融合角度=陀螺权值*陀螺积分角度+(1-陀螺权值)*加速度角度；还有一种叫最大梯度法的，就是你发的这段代码

      // integral error scaled integral gain
         exInt = exInt + ex*Ki;
         eyInt = eyInt + ey*Ki;
         ezInt = ezInt + ez*Ki;


         // adjusted gyroscope measurements
         gx = gx + Kp*ex + exInt;
         gy = gy + Kp*ey + eyInt;
         gz = gz + Kp*ez + ezInt;

大概是这样的： 陀螺积分角度+=角速度*dt；角度偏差=加速度角度-陀螺积分角度；融合角度=陀螺角度+衰减系数*角度偏差；用来矫正陀螺本身的静态漂移：角度偏差积分+=角度偏差；角速度=角速度+衰减系数*角度偏差积分；这个就可以一定程度上矫正陀螺的零漂。

 下面这幅图才是准确的阐述了互补滤波的过程。正常情况下用陀螺仪的数据就可以进行姿态的更新，但是由于陀螺仪的积分误差，这里用acc和mag去校正，求出他们的误差用PI去弥补。注意看看pid的公式和作用，pid是作用于误差（实际个期望之间的差值），最终反复调节，让实际值=期望值。



互补滤波——加速度计滤掉高频信号，陀螺仪滤掉低频信号，互补滤波器就是根据传感器特性不同，通过不同的滤波器（高通或低通，互补的），然后再相加得到整个频带的信号。


  参考博客
 http://blog.csdn.net/wkdwl/article/details/52119163

http://blog.csdn.net/Gen_Ye/article/details/52522721?locationNum=10

http://blog.csdn.net/Gen_Ye/article/details/52524190