互补滤波（complementary filter）
2007年，MIT 大神 Shane Colton 发表了经典论文《The Balance Filter》，里面提出了一种对加速度传感器与陀螺仪进行数据融合的有效方法——互补滤波。互补滤波包括低通滤波（滤除或衰减短期加速度传感器波动），以及高通滤波（消除漂移对陀螺仪的影响）。
在加速度传感器部分，我们通过反正切函数求出了角度，这里用 Acc 表示；在陀螺仪部分，我们得到了角速度，这里用 Gyro表示。
互补滤波的核心公式为：
Angle=0.98∗(Angle+Gyro∗dt)+0.02∗Acc
其中，
Angle 为经过互补滤波后得到的角度
Gyro 为陀螺仪部分得到的角速度
Acc 为加速度传感器部分通过反正切函数 atan() 再转换单位后的角度
dt 为滤波器的运行周期
0.98 和 0.02 为加权系数 α 和 (1−α)
使用 c 语言把互补滤波封装成函数：
// a = tau / (tau + dt)
// acc = 加速度传感器数据
// gyro = 陀螺仪数据
// dt = 运行周期
float angle;
float a;
float ComplementaryFliter(float acc, float gyro, float dt)
{
a = 0.98;
angle = a * (angle + gyro * dt) + (1 - a) * (acc);
return angle;
}
在互补滤波公式中，像陀螺仪角速度、加速度角度这些数据可以直接从陀螺仪、加速度传感器中读取得到。可是，加权系数是怎么得来的呢？
在《The Balance Filter》中提到关于 加权系数的求解公式。在这里，先设滤波器的加权系数为 α，时间常数为为 τ，运行周期为 dt，那么公式为：α=τ/(τ+dt)
运行周期 dt 通常都是我们设定的。比如在程序里，我们打算让互补滤波器在 1s(=1000ms) 中运行 200 次，那么dt=1000ms/200hz=5ms。
时间常数 τ 的取值是由我们根据系统的实际滤波需求调整的，每个不同的系统的 τ 值都不一定相同。时间常数 τ 是“信任”陀螺仪和“信任”加速度传感器的边界值。若时间常数 τ 取值越大，则更加“信任”陀螺仪积分，但跟随加速度传感器的速度会变慢。若时间常数 τ 取值越小，则更加“信任”加速度传感器，但同时引入加速度传感器中更多的噪声。通常，互补滤波器是为了得到更“纯”的融合角度，必须要“信任”陀螺仪积分多些，以削弱加速度传感器中噪声的影响。
比如，互补滤波器放在 10ms(=0.01s) 周期中循环运行，时间常数 τ=0.49，那么，此时加权系数为：
α=τ/(τ+dt)=0.49/(0.49+0.01)=0.98
函数的入口参数是加速度传感器数据、陀螺仪数据和运行周期。经过计算后，函数返回融合角度。
为了更进一步了解加权系数的作用，我们留意两个极值情况：
当a=1 时，那么滤波器变成 angle=1∗(Angle+Gyro∗dt)+(1−1)∗Acc=Angle+Gyro∗dt，失去了加速度传感器的修正效果，变成了一个纯粹的陀螺仪积分器，此时互补滤波器完全信任陀螺仪。
当a=0 时，那么滤波器变成 angle=0∗(Angle+Gyro∗dt)+(1−0)∗Acc=Acc，此时陀螺仪完全失去了作用，融合角度直接等于加速度传感器数值，此时互补滤波器完全信任加速度传感器。
由此可见，加权系数 α 值的大小决定了互补滤波器是信任陀螺仪还是信任加速度传感器多一些，也直接决定了互补滤波器的效果。
当然，互补滤波器也存在缺陷：
初始化时不能及时跟随实际角度。这种情况，跟融合角度变量没有正确初始化和互补滤波器的时间常数取值有关。时间常数约大，陀螺仪积分比重越大，跟随加速度传感器的速度约慢，这意味着初始化时融合角度可能是不精准的。
陀螺仪的零漂问题。理论上，陀螺仪在静止的时候，读出来的数值应该为 0，但实际上往往不为 0，这个值被称为零偏值。陀螺仪每次上电后的零偏值是随机的。互补滤波器并不能有效地过滤掉陀螺仪的零漂。
