飛控IMU數據進階處理（FFT，濾波器）
前面的文章曾簡單講過IMU數據（陀螺儀、加速度數據）的校準以及一階低通濾波。本文在此基礎上更進一步講一下數據的指標分析與濾波器的選擇問題。
IMU數據的重要性
IMU數據在飛控中處於最底層的數據，其重要性可見一斑。一般可降級的飛控，在遇到如GPS故障時，可降級飛行模式，來保證飛行安全性。其中，由陀螺儀數據決定的角速度飛行模式是所有飛行模式的基礎，換而言之，一個飛機飛的穩不穩主要要看陀螺儀的數據。
採樣頻率
前面的文章有提到過控制帶寬的問題，一般來說，由於電調的響應在400hz左右，所以設計的角速度控制環最高可到400hz左右（再高就沒有什麼意義了）；而一般飛行器的帶寬在30-40hz左右，所以控制頻率最好不低於200hz。所以，角速度的反饋數據，頻率最好在200-400hz左右（至少與控制頻率一致）。
頻率混疊
如果IMU的採樣頻率降到與幹擾頻率差不多時，比如IMU的採樣為200hz，而實際可能有192hz的高頻幹擾，因為採樣只有200hz，只能分辨出100hz的數據，這時192hz的幹擾會對數據造成頻率混疊，即低頻段的數據波形錯誤，數據失真。
結論：數據的採樣頻率儘可能高，這裡既有控制器的考慮，也避免頻率混疊的情況。
如何減震？
有過飛機組裝設計經驗的人知道，飛機在飛行時，由於電機的轉動，導致機身的震動過大，而這些震動對飛控的數據影響很大，所以必須要做一定的減震處理（實際有些飛行器可能不需要減震，由各個飛機的情況決定）。
而最頭痛的就是如何設計減震？或者說，怎麼判定減震是否設計良好？由於最終目的是處理好IMU的數據，所以就飛控而言，即通過IMU數據的質量好壞來辨別是否減震設計良好。
如何分析IMU數據質量？
時域上來看的話，取一組懸停飛行的波形觀察，經驗值是陀螺儀的數據雜訊波動不超過正負0.15rad/s，加速度不超過正負3m/s2。
筆者這裡取出一組飛行良好的數據，200hz採樣，200hz回傳保存。飛行動作包括懸停與roll軸的動作。數據包含兩個部分，200hz採樣的原始數據，和進行50hz的巴特沃斯低通濾波後的數據。數據波形如下：
放大懸停部分：
圖中可以看出，原始數據噪音很大，波動範圍約正負0.6rad/s，而紅色濾波後的數據則表現良好。
對數據進行FFT分析：
圖2可以看到原始數據在80-100hz左右具有高頻幹擾，而圖3，濾波後的FFT表明這段高頻幹擾已經去除，低頻段的波形基本一致，證明濾波器有效。
濾波器截止頻率選擇
如上數據波形所示，對原始數據進行FFT分析後，得到其各個頻段的幅值，比如這裡的80-100hz有幹擾，則將濾波器設置在70以下都可以。
結論：濾波器截止頻率的選擇與原始數據的FFT有關。通常有個經驗值30-50都可。
濾波器是否有差別？
這裡嘗試三個不同的濾波器，一階低通、2階IIR和FIR。
設計一個6階的FIR濾波器，截止頻率50hz，對比二階巴特沃斯，50hz濾波器，以及50hz一階低通。
對比濾波後的FFT，放大觀察
可見6階FIR濾波器濾波效果更佳，高頻段幾乎濾掉。所以6階FIR > 2階IIR > 一階低通
IIR與FIR的區別？
簡單來說就是一個是非線性相位延遲，一個是線性相位延遲。比如FIR，無論在低頻還是高頻段，其濾波後的數據延遲是固定的。而IIR則不是，在低頻段的延遲小於其高頻段的延遲。
為什麼開源用的都是IIR？
這裡筆者分析主要是實現問題，IIR結構簡單，只需在代碼中設置好採樣頻率和截止頻率，則自動生成對應的濾波器參數，而FIR這裡，筆者是在matlab下設計的濾波器，再將得到的參數放置在代碼中，過程較為繁瑣，且優勢不明顯。另外，IIR濾波器的運算量也較FIR要小。
其次，這裡，在動態下，6階FIR的延遲較大，所以濾波時不能光考慮數據的平滑，還要考慮延遲對控制器的影響，綜合來看，還是IIR較為好用。
數據延時約0.01s。
所以，綜上，一般採用2階IIR濾波即可，其在數據的平滑性和可接受的延遲兩個指標下綜合最優。
延時有處理辦法嗎？
如果基於上面FIR濾波過後的數據，數據質量良好，但是延時過大，如何處理？
給數據加上微分量作為預測。
放大觀察
這裡延時是處理了，但是犧牲了數據的質量，數據的幅值發生變化，因此該處理是有嚴格條件的。實際應用中，並不一定好用。
卡爾曼濾波
這裡筆者設計了一個單維的卡爾曼濾波，模型如下：
解釋一下，認為狀態轉移為1，即數據無變化，u為模型的輸入，以角速度的微分即角加速度。觀測值即模型的輸出，所以C也為1.
時域
放大觀察
該模型下kalman，其性能與2階IIR幾乎一致。無特別明顯優勢處。
總結：
? 濾波器的截止頻率選擇由FFT分析決定；
? 關於濾波器的優劣：綜合來看2階IIR佔優，但不排除其他的特殊處理，比如延時處理後的FIR濾波等；
? 加速度的分析與此處舉例的陀螺儀類似；
推薦閱讀：
