狀态估計公開課--基于卡爾曼濾波的應用(多傳感器資料融合)

其實狀态估計包含兩個部分：

姿态估計

位置估計

姿态指某一物體的在三維空間的方向指向，位置是指GPS坐标系或者NED坐标系下的位移。一個無人系統，不管是無人車、無人機、無人船或是機器人等，要在三維空間自由運作，該系統的姿态估計和位置估計都必須非常準确。就像人腦可以用眼睛和大腦辨識方位一樣，假如蒙住雙眼，我們走路都走不出直線。同理如果無人系統的姿态估計和位置估計不準确，那麼無人系統在三維空間是沒辦法自由工作的。

是以，在無人系統中非常重要一部分工作就是：完成準确的姿态估計和位置估計，這兩部分是無人系統的基石。姿态估計有問題，多旋翼無人機将飛行不穩定，影響姿态控制。位置估計有問題，多旋翼無人機将不知道目前準确的位置，影響位移精度，或者把地面當作天空而墜毀。在無人車系統中位置估計不準确，将沒辦法避開障礙物。

狀态估計的真實值和估計值

真實值的定義：

搜尋了一下，官方解答就是：指被測物質的真實量，嚴格地說，任何物質的真實含量是不可知的，但人們可采用各種可靠的分析方法，經過不同實驗室、不同人員反複進行測定，用數理統計方法，最後得出公認的測量值，該值便可用以代表被測物的真實值。由以上的定義貌似看出，人們通過各種方法去追求的真實值，其實隻是最接近真實狀态的一個近似真實值。現實生活中，我們為了簡單友善，大多數是根據少量的測量值或者以往的經驗，對某個物質的真實狀态進行估計。

而估計的定義：

wiki的解釋是“Estimation theory is a branch of statistics that deals with estimating the values of parameters based on measured empirical data that has a random component. ”。用谷歌翻譯成中文的是“估算理論是統計學的一個分支，它處理基于具有随機分量的測量經驗資料估計參數值”。而最優估計，是指在某種特定的準則下達到最優；狀态估計，既是對動态随機系統的估計。綜合起來，最優狀态估計，根據某種特定的估計準則對真實的狀态估計達到最優。

說了這麼多廢話，那麼狀态估計在自動控制，航空航天，通訊和工業生産，SLAM中的具體作用是什麼呢？他到底有多大的魔力呢？他又會涉及到哪些方方面面的理論知識呢？

我還是首先說一下他會涉及到的有哪些理論知識吧，當然是離不開我們從國小到大的數學知識，矩陣運算，機率運算。

首先是在20世紀30年代，随着随機過程的估計問題展開，美國學者維納提出的在頻域中設計統計最優濾波器的方法，該方法被稱為維納濾波。但是該方法局限于處理平穩的随機過程，并且隻能提供穩态的最優估值，是以在實際工程的應用中受到了很多局限。在20世紀60年代，美國學者卡爾曼和布西提出了最優遞推濾波方法，這一方法，被稱為kalman濾波。卡爾曼濾波考慮了被估量和觀測量的統計特性，适用于平穩随機過程和非平穩随機過程，并且可用數字計算機來實作，是以，kalman濾波方法得到了廣泛的應用。

Kalman的應用，最引以為豪的是卡爾曼在一次通路NASA埃姆斯研究中心時，發現kalman濾波能幫助解決阿波羅計劃的軌道預測問題，後來NASA在阿波羅飛船的導航系統中确實也用到了該濾波器。最終，飛船正确駛向月球，完成了人類曆史上的第一次登月。

當然，現在卡爾曼濾波的應用不僅停留在阿波羅登月上，在電力系統中，由于可再生能源輸出功率、負荷變化以及其他随機過程的存在,使得系統狀态參數中往往混雜着噪聲，是以有必要采取适當的方法，從随機幹擾的觀測信号中提取有效的系統狀态參數。卡爾曼濾波及其擴充形式在電力系統短期負荷預測、動态狀态估計、電能品質分析、繼電保護、風電場風速預測、電機狀态和參數估計等方面的應用。

無人駕駛汽車對道路環境與車體狀态的感覺一般通過GPRS、雷達、錄影機、陀螺儀等獲得，測量過程往往帶有噪聲。Kalman濾波進行車道線的預測跟蹤，通過對目前圖像預測估計下一時刻車道線的位置資訊，并且能識别判斷出車道線的走向(左轉或右轉)，提取有效的道路資訊進行車道線的跟蹤。帶有Kalman濾波的最優預測控制器，與不考慮噪聲幹擾的最優預測控制器相比，會産生比較平滑的控制序列，能夠減少了機械損耗。

在無人機中的應用，如目前開源的PX4/ecl庫的EKF，具有24維狀态，其優點有:

1>對測量資料通過buffer進行存儲處理，将具有不同時延和資料速率不同的傳感器的資料在數學上達到同一時刻，則提高了狀态的估計精度。

2>可以同時融合不同的傳感器資料，如GPS，IMU資料，超音波/毫米波資料，氣壓計高度，地磁資料，RTK航向等。

3>在對資料進行融合的過程中，根據統計資料的顯著不一緻性，報告傳感器資料的錯誤，診斷出傳感器故障。

4>在固定翼中，可以不需要風速計估計出風速，以延長GPS在飛行過程中丢失時的航位推算時間。

5>估計3軸加速度計偏差，這提高了TailSitter和其他飛行器在飛行階段之間經曆大姿态變化的精度并且修正傳感器的bias。

6>聯合姿态與位置/速度估計姿态，提高了姿态估計精度和提升了其魯棒性。

對于卡爾曼濾波，我們幾乎可以下這麼一個定論：

隻要是存在不确定資訊的動态系統，卡爾曼濾波就可以對系統下一步要做什麼做出有根據的推測。即便有噪聲資訊幹擾，卡爾曼濾波通常也能很好的弄清楚究竟發生了什麼，找出現象間不易察覺的相關性。

是以卡爾曼濾波非常适合不斷變化的系統，它的優點還有記憶體占用較小（隻需保留前一個狀态）、速度快，是實時問題和嵌入式系統的理想選擇。

為了大家可以深入了解基于卡爾曼濾波的狀态估計，我們特别開設了本次的狀态估計公開課。

注：關注微信公衆号：阿木實驗室，回複【狀态估計公開課】即可免費領取視訊課程