爲什麼開源代碼看不懂？
說起互補濾波，之前非常的流行，在那個算力不夠的年代，這種短小精幹的融合算法，風靡一時。
原理也非常簡單：
我有兩路信號，一個帶有高頻噪聲，一個帶有低頻噪聲，所以我把他們的噪聲分別濾除，然後合併，就得到了沒有噪聲的原始信號。
簡單直接，清晰明瞭，看到這你頓時覺得這個算法你已經會了，甚至連算法的流程都設計好了。
於是你在網上找到了著名的 Madgwick實現的 MahonyAHRS 算法，準備對比一下自己的思路。
function obj = UpdateIMU(obj, Gyroscope, Accelerometer)
q = obj.Quaternion;
if(norm(Accelerometer) == 0), return; end
Accelerometer = Accelerometer / norm(Accelerometer);
v = [2*(q(2)*q(4) - q(1)*q(3))
2*(q(1)*q(2) + q(3)*q(4))
q(1)^2 - q(2)^2 - q(3)^2 + q(4)^2];
e = cross(Accelerometer, v);
if(obj.Ki > 0)
obj.eInt = obj.eInt + e * obj.SamplePeriod;
else
obj.eInt = [0 0 0];
end
Gyroscope = Gyroscope + obj.Kp * e + obj.Ki * obj.eInt;
qDot = 0.5 * quaternProd(q, [0 Gyroscope(1) Gyroscope(2) Gyroscope(3)]);
q = q + qDot * obj.SamplePeriod;
obj.Quaternion = q / norm(q);
end
你看着開源的代碼和自己畫的流程圖，反覆對照了20遍，突然想到了自己上數學課的樣子。
於是你又上網看了無數的互補濾波解讀教程，始終不理解，爲什麼算法原理和代碼可以沒有任何關係？，那這個算法是怎麼寫成代碼的呢？
我直接給出結論吧，造成這樣的原因是因爲：
網上大部分互補濾波原理介紹的是傳統的線性互補濾波(Classical Complementary Filters)，
而Mahony用來算解姿態的濾波是經過改進的非線性互補濾波，
非線性互補濾波里有兩種形式：直接互補濾波(Direct complementary filter)和無源互補濾波(Passive complementary filter)，
你在網上看到的開源代碼都是基於無源互補濾波器的顯式誤差版本-顯式互補濾波器(Explicit complementary filter).
你拿着兩個完全不一樣的東西，那肯定對應不上呀。
先從傳感器開始說起
但是我就納悶了，爲啥就沒人循序漸進的寫清楚呢？既然沒人那就我來吧。
我有兩路姿態，一個帶有高頻噪聲，一個帶有低頻噪聲，所以我把他們的噪聲分別濾除，然後合併，就得到了沒有噪聲的姿態信息。
我有哪兩路姿態信息呢？
但凡你在網絡上查過資料，他們一定會告訴你，一路陀螺儀的獲取的姿態，一路加計獲取的姿態。
真的是這樣嗎？
那我們先從陀螺儀開始吧，陀螺儀可以獲得三軸角速度，積分一下就可以得到角度，在小角度假設下，這個角度可以認爲就是姿態角。
思路沒有任何問題，只是我們可能缺個初始狀態。
我們求速度的時候是：
那我們求角度的時候自然是：
所以如果我們卻個初始狀態angle0，陀螺儀只有在發生轉動的時候纔能有數據，所以它是無法提供這個初始狀態angle0的。
沒關係,起飛前的初始狀態非常好分析，因爲這時候飛機是受力平衡的，也就是說在地理系下的加速度的讀數是[0;0;g]
所以我們可以列出方程：
得到：
那麼根據加計得到的初始的roll，pitch角度正確時，我們可以通過公式，得到yaw：
這樣，我們有了angle0 和 w ,終於和通過陀螺儀來獲取姿態了。
ps:詳細推導思路參考：《基於加速度計與磁力計的姿態解算方法》
等等，這樣的話我們不就只有一路輸入了嗎？怎我三個傳感器用完才能得到一路姿態呀？
加計不是可以求姿態嗎？這時我纔想起來，網上流傳的加計求姿態公式，其實只能在飛機受力平衡的時候使用，當飛機受力不平衡，飛機在地理系受到的加速度不是[0;0;g]，而是個未知數，上面的方程是沒有解的。
連兩路輸入都沒有，還怎麼互補濾波？
必要的假設
所以在討論互補濾波器之前我們要做出幾個假設：
1.姿態的更新是線性的即滿足公式
2.飛行過程基本受力平衡，接近勻速直線運動，或者懸停，即飛機在地理系下的加計讀數爲[0;0;g]
3.傳感器的測量數據只涉及高頻或者低頻噪聲，即，傳感器測量方程如下：
m下標爲傳感器測量值，等於真實值加上噪聲，所以可以推導出，傳感器測量出的角度也滿足以下測量方程。
4.假設初始歐拉角爲[0;0;0]
所以傳統的線性互補濾波結構如下。
從低通濾波器開始分析
低通濾波器是我們比較熟悉的，之前我們分析過一階低通濾波器，但是低通濾波器有很多種，爲了討論不同的狀態，令低通濾波器函數爲LPF。
那高通濾波器是什麼呢？這裏我們的低通濾波和高通濾波合併後希望能夠通過完整的波形，也就是波形完全不變，那這個全通的函數其實就是1，所以我們高通濾波器就可以設計爲1-LPF。
以我們最熟悉的一階低通濾波器爲例，它的函數爲：
那高通濾波就是：
那這個結論對不對呢？令截止頻率wc=1HZ，畫出兩個函數的伯德圖，完全符合預期，一個低通一個高通且截止評率1hz.
ps:一階濾波器詳細分析參考《一階RC低通濾波算法》
我們再試試二階濾波器，二階低通濾波器函數爲：
二階高通濾波器爲：
令所有係數爲1，a1=a2=a3=b1=b2=b3=1,畫出濾波器的伯德圖
結果也是符合預期的。
所以，可以看見很過論文把這個過程進行了總結，給出了通用的低通濾波函數：
當C(s) = 常數時，對應的就是一階濾波器。
當C(s) = a+ b/s 時，對應的就是二階濾波器 。
(二階的這個形式正好和KP，Ki形式一致，很多地方都會強調這裏的KP，Ki有什麼做用，又是消除誤差，又是調整截止頻率，但是我認爲在線性互補濾波器中，這裏的參數的作用就是構成二階濾波器，這裏的參數也僅僅是二階濾波器的參數)
所以我們也用這種通用形式來繼續分析。
根據框圖可得：
是不是有內味了，再根據這個公式，把框圖化簡成反饋形式。
這就是論文裏經常看見的框圖，我們來對比一下
是不是一模一樣。
那爲什麼要把它轉變成反饋形式呢？這種反饋結構簡化了濾波器在代碼上的實現過程，同時可以地適應更多的測量，可以很方便的擴展結構如下圖：
什麼時候會需要擴展呢？例如當你有GPS的時候：
算法實現
ok,我們終於把線性互補濾波器的原理說完了，那就把它實現了吧。
之前我們已經很詳細的分析過低通濾波器了，這次還是以一階濾波器的形式實現。
令C=1/τ
一階向後差分：
離散化
轉換爲離散時間的差分形式
截止頻率爲：
用MATLAB實現算法如下：
function obj = UpdateIMU(obj, w_m,angle_m )
obj.angle = (obj.tao/(obj.tao + obj.Ts)) * ( obj.last_angle + obj.Ts*w_m)
+ (obj.Ts/(obj.tao + obj.Ts)) * angle_m;
obj.last_angle = obj.angle;
end
仿真對比
找一組數據驗證一下：
可以看到直接通過加計得到的圖形確實噪聲很大，符合加計容易受噪聲影響的特點。
零初始狀態只用陀螺儀積分得到的陀螺儀，則非常平滑，無法感知到高頻部分的數據，但是靜止時得到的角度有偏離，不是0度，符合陀螺儀動態響應效果好的特點。
傳統互補濾波器和Mahony濾波器確實都能夠結合兩者的特點，在15s處能得到更多的姿態信息。
ps:Mahony算法和測試數據來源於https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/
思考
讓我們再看一眼，濾波器的框圖：
這個框圖可以很直接的看到，核心思路就是如果獲取一個更精確的角速度，因爲本質上我們想使用的就是角速度積分的到姿態，而角速度積分得到姿態就是姿態更新的線性化過程，我想這就是爲什麼這叫線性互補濾波器的原因，因爲一切的起因都這個線性姿態更新算法。
關於經典的線性互補濾波器的原理，算法，結果測試已經全部講完了，下一次我們就要開始非線性互補濾波器了，算法的改進往往是把之前算法的假設修改的更貼近與實際。
ok，今天就講這麼多，我是zing,一個有趣的飛控算法工程師，我們下期見。
PS：所有的論文資料和測試代碼，公衆號【zingsay】回覆【互補濾波】即可獲取。
