01 卡爾曼濾波是什麼？
Kalman濾波是在時域上運用狀态空間，遞推得到的一種濾波算法，便于在計算機上實時實作，計算量和存儲量小。該方法可處理多變量非平穩随機過程濾波問題，可處理時變系統濾波問題。例如飛機在飛行過程中，遇到的幹擾通常是時變非平穩的噪聲，此時運用卡爾曼濾波可有效去除幹擾，得到較真實的狀态估計資料。
02 卡爾曼濾波在飛控系統中的作用？
飛機在飛行過程中，其飛行的俯仰橫滾角度與航向角度通常磁力計與陀螺儀提供，飛機的位置與速度通常由加速度計通過機理模組化得到，簡單來說飛機的角度與位置速度等資料可以由傳感器和飛控機理算法得到，但是由于飛機在飛行過程中會有噪聲輸入，此時狀态方程與觀測方程會有一定的噪聲輸入，那麼回報的資料就是模糊不準确的，如何去除掉這些外來的幹擾噪聲，讓飛機得到真實的飛行資料呢？那麼卡爾曼濾波就可以實作這樣的功能。
03 舉例說明：
如圖（1）所示，飛機在飛行過程中有外來噪聲輸入，傳感器采集到有幹擾的資料送給濾波估計器，濾波器負責去除這些噪聲還原飛機真實的資料送給控制器，控制器得到飛機的真實資料，控制輸出到飛機。這樣飛機有了真實的資料，就可以更好的控制飛機，達到穩定飛行的目的。
圖（1）
- 卡爾曼濾波公式
Q與R為w(t),v(t)的方差陣；P為狀态估計方差陣。
注：所引用公式出自黑龍江大學鄧自立教授所著《模組化與估計》p126-p129。
- 卡爾曼濾波簡化
将卡爾曼濾波公式（2）的前三步進行簡化合并，得到如下公式：
- 卡爾曼濾波遞推算法框圖
- 卡爾曼濾波遞推算法框圖簡化
Matlab中卡爾曼濾波實作
以下初步講解卡爾曼濾波在Matlab中仿真實作，所用資料均來自阿木實驗室-鉑貝學院出品的《卡爾曼濾波（理論）》第一節課，有興趣的讀者可以一步觀看視訊講解，第一節課為試聽免費課程。（鉑貝學院：https://bbs.amovlab.com/plugin.php?id=zhanmishu_video:video&mod=video&cid=19）
◆如圖（2）圖（3）所示，以小車的位置速度為狀态估計目标，由位置與速度的實體機體關系，對小車模組化，得到狀态估計方程；對小車的傳感器模組化，得到觀測方程。
圖（2）
圖（3）
◆簡化卡爾曼濾波，得到如圖（4）所示的公式。
圖（4）
◆在matlab中編輯代碼實作。代碼如下：
◆觀察狀态估計輸出
小車的位置速度初始狀态為0，有方差為1的噪聲輸入時，小車的位置速度便不為0，此時由卡爾曼濾波估計器得到的位置速度估計如下圖（5）。
圖（5）輸入方差為1的噪聲
當輸入方差為10的噪聲時，位置速度估計如圖（6），與圖（5）比較，可知輸入噪聲越大，對于系統穩定性影響越大。
圖（6）輸入方差為10的噪聲
本文關于卡爾曼濾波的注解均為個人了解，有錯誤之處請批評指正。
作者：阿木實驗室研發部-孫遠航
歡迎各位助教參與投稿~