爲什麼開源代碼看不懂?
說起互補濾波,之前非常的流行,在那個算力不夠的年代,這種短小精幹的融合算法,風靡一時。
原理也非常簡單:
我有兩路信號,一個帶有高頻噪聲,一個帶有低頻噪聲,所以我把他們的噪聲分別濾除,然後合併,就得到了沒有噪聲的原始信號。
簡單直接,清晰明瞭,看到這你頓時覺得這個算法你已經會了,甚至連算法的流程都設計好了。
於是你在網上找到了著名的 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】回覆【互補濾波】即可獲取。