????2007年，MIT 大神 Shane Colton 發(fā)表了經典論文《The Balance Filter》，里面提出了一種對加速度傳感器與陀螺儀進行數(shù)據(jù)融合的有效方法——互補濾波?；パa濾波包括低通濾波（濾除或衰減短期加速度傳感器波動），以及高通濾波（消除漂移對陀螺儀的影響）。

????在加速度傳感器部分，我們通過反正切函數(shù)求出了角度，這里用 Acc 表示；在陀螺儀部分，我們得到了角速度，這里用 Gyro表示。

互補濾波的核心公式為：

Angle=0.98?(Angle+Gyro?dt)+0.02?Acc

其中，

• Angle 為經過互補濾波后得到的角度

• Gyro 為陀螺儀部分得到的角速度

• Acc 為加速度傳感器部分通過反正切函數(shù) atan() 再轉換單位后的角度

• dt 為濾波器的運行周期

• 0.98 和 0.02 為加權系數(shù)?α?和?(1?α)

使用 c 語言把互補濾波封裝成函數(shù)：

// a = tau / (tau + dt)??

// acc = 加速度傳感器數(shù)據(jù)?

// gyro = 陀螺儀數(shù)據(jù)?

// 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;??

}

在互補濾波公式中，像陀螺儀角速度、加速度角度這些數(shù)據(jù)可以直接從陀螺儀、加速度傳感器中讀取得到?？墒牵訖嘞禂?shù)是怎么得來的呢？

在《The Balance Filter》中提到關于 加權系數(shù)的求解公式。在這里，先設濾波器的加權系數(shù)為?α，時間常數(shù)為為?τ，運行周期為?dt，那么公式為：α=τ/(τ+dt)

運行周期?dt?通常都是我們設定的。比如在程序里，我們打算讓互補濾波器在?1s(=1000ms)?中運行 200 次，那么dt=1000ms/200hz=5ms。

時間常數(shù)?τ?的取值是由我們根據(jù)系統(tǒng)的實際濾波需求調整的，每個不同的系統(tǒng)的?τ?值都不一定相同。時間常數(shù)?τ?是“信任”陀螺儀和“信任”加速度傳感器的邊界值。若時間常數(shù)?τ?取值越大，則更加“信任”陀螺儀積分，但跟隨加速度傳感器的速度會變慢。若時間常數(shù)?τ?取值越小，則更加“信任”加速度傳感器，但同時引入加速度傳感器中更多的噪聲。通常，互補濾波器是為了得到更“純”的融合角度，必須要“信任”陀螺儀積分多些，以削弱加速度傳感器中噪聲的影響。

比如，互補濾波器放在?10ms(=0.01s)?周期中循環(huán)運行，時間常數(shù)?τ=0.49，那么，此時加權系數(shù)為：

α=τ/(τ+dt)=0.49/(0.49+0.01)=0.98

函數(shù)的入口參數(shù)是加速度傳感器數(shù)據(jù)、陀螺儀數(shù)據(jù)和運行周期。經過計算后，函數(shù)返回融合角度。

為了更進一步了解加權系數(shù)的作用，我們留意兩個極值情況：

• 當a=1?時，那么濾波器變成?angle=1?(Angle+Gyro?dt)+(1?1)?Acc=Angle+Gyro?dt，失去了加速度傳感器的修正效果，變成了一個純粹的陀螺儀積分器，此時互補濾波器完全信任陀螺儀。

• 當a=0?時，那么濾波器變成?angle=0?(Angle+Gyro?dt)+(1?0)?Acc=Acc，此時陀螺儀完全失去了作用，融合角度直接等于加速度傳感器數(shù)值，此時互補濾波器完全信任加速度傳感器。

由此可見，加權系數(shù)?α?值的大小決定了互補濾波器是信任陀螺儀還是信任加速度傳感器多一些，也直接決定了互補濾波器的效果。

當然，互補濾波器也存在缺陷：

• 初始化時不能及時跟隨實際角度。這種情況，跟融合角度變量沒有正確初始化和互補濾波器的時間常數(shù)取值有關。時間常數(shù)約大，陀螺儀積分比重越大，跟隨加速度傳感器的速度約慢，這意味著初始化時融合角度可能是不精準的。

• 陀螺儀的零漂問題。理論上，陀螺儀在靜止的時候，讀出來的數(shù)值應該為 0，但實際上往往不為 0，這個值被稱為零偏值。陀螺儀每次上電后的零偏值是隨機的。互補濾波器并不能有效地過濾掉陀螺儀的零漂。