淺談卡爾曼濾波

文章以最小二乘法為切入點展開討論卡爾曼濾波

。卡爾曼濾波與最小二乘法只有很小的區別。

經典最小二乘法解決的問題：考慮下面的個測量：

每次測量都有自己的誤差. 不可觀測， 。是適當維度的向量。最小二乘法通過最小化下面的式子來獲得對最有可能的估計：

這產生了一個問題，那就是每次測量誤差都不一樣，不能一概而論，如果每次測量都同樣的權重。會導致結果被某些誤差比較大的數據帶偏。（其實多數情況下誤差分佈也是不知道的），所以更通用的形式是帶權最小二乘法：

其中:

考慮一個有點複雜的問題：

兩架飛機我們想通過在中觀察到的位置信息，近似得到速度，加速度，等信息。

而往往我們自身的狀態也在改變。我們對自身狀態的改變也是一個帶有雜訊的測量。

這一過程可以表示為：

是不相關，均值為的雜訊。一般記： 表示對 估計

這裡，如果我們單看式，這其實是一個最小二乘法可以解決的問題。

式子對應觀測數據。比如第次測量的相對位置，也就是則可以驗證。理想情況下，我們估計的會有： .往往測量是有誤差的。而在驗證之前，因為上一時刻的速度，位置等狀態信息與現在有改變，導致測量的結果與狀態的改變有關。要通過 式子對歷史 進行狀態轉移。這相當於不同參考系觀測的數據要處理到同一個參考系。這是因為一個人看到另一個人速度的快慢也跟自身速度有關。

式是一組狀態轉移，是時刻，系統的一個輸入。則是對上一時刻狀態的轉移。例如，上一時刻觀察到的位置信息到當前的變換。這裡理解為參考系狀態的改變。比如產生了一個加速。改變了你觀測的相對加速度。

卡爾曼濾波將會以下面的方式來估計狀態：

上圖中，假設只能得到相對於自身的位置和兩次測量之間的時間間隔，以及自身位置速度，加速度等信息。

黃色箭頭表示我們對的速度，加速度等狀態的估計。由於的時刻到與時刻速度，加速度，位置等信息的不同，所以需要先通過狀態轉移對歷史估計轉移到新的估計。再利用最新的位置來矯正轉移後新的估計。獲得時刻狀態的估計。

如果沒有的狀態轉移，可以直接最小二乘法通過極小化目標函數來求。

注意：此時可以假設 相當於靜止的。且 勻加速運動，觀測信息 是總時間的轉移矩陣而不是時間間隔。你可以認為 .這個舉例只是你便於理解上面 組成的系統，也可以不用參考。由於 的靜止，我門忽略掉 .此時就是最小二乘法可以解決的問題。

最小二乘法

其中，

其中：

對每一次觀測進行歸一化.（這裡，其實默認假設了 )

你也可以認為表示了權重，即帶權最小二乘法（其實權重可以不僅僅限於）

為什麼權重要是呢。單看式，不同時刻誤差的大小，讓我們對某些測量結果不是那麼相信。但雖然有不好的測量，但也一定程度上反映了真實的結果。經過歸一化後，每次測量誤差都變成了均值為，方差為。而方差相同，意味著兩次測量的可信度相同了。

這裡其實假設了我們已經知道了測量誤差的方差，但現實中我們往往是不知道的。把它假設出來，只是為了便於數學上的說明。如果不知道，那麼大可認為方差都是一樣。此時第一直覺是設置一個常數。其實大可認為是 。因為此時不管取何數值，都不會影響最終結果。因為各項權重都是一樣的。

那麼一般最小二乘法中， ,按照帶權最小二乘法來說，我們默認了

經典的求導,令：

則：

一般會記:

正如標題曲線擬合那樣，最小二乘法曲線擬合是不會隨時間變化的。而式組曾的系統卻反應出的是一個會變的 ,也就是說，是有區別的。所以與兩者測量的已經不一樣了。

下面你會看到卡爾曼濾波如何將最小二乘法應用到上面 組成的系統中。

協方差矩陣

注意： ，即為的協方差矩陣。這是卡爾曼濾波的關鍵。

證明：

其中：

解釋一下：

當我們知道誤差時，這其實就是確定性求解。

則：

這裡，， ,這是因為，不同時刻的測量誤差是不相關的。

注意：到這裡我們只需要知道了協方差矩陣即可立即有：

進一步令

得到一組遞推關係。將協方差轉為在線計算。

協方差傳遞

注意，之所以要傳遞協方差很容易理解。上一時刻與當前時刻的狀態是不同。比如參考系 位置，速度，加速度的改變。

由於就是協方差的這一性質，那麼當經過式，狀態轉移時，協方差也可以傳遞。

對於：

解釋一下，對於前面的式子中，都是在最小二乘法的背景下來討論的。那個時候，是不隨時間變化的。而在加入的狀態轉移後，狀態的轉移，使得時刻的與時刻的有所不同。所以要有下標來做時間上的區分。

那麼：

協方差傳遞矩陣為：

其中： .

這樣就方便我們將最小二乘法擴展到 所組成的系統。

卡爾曼濾波

表示在不參考與時，系統對的估計。

表示參考了後，系統對的估計。

這裡，為了區分矩陣的逆。用上標 表示先驗。

但，此時 這個操作是不對的，按照來說，因為參考系在不斷的運動。觀察到的數據兩個時刻差異可能很大。但被觀察的卻是一個勻加速運動。這就好比你 過去的觀察數據 和 現在的觀察數據 參考系的狀態一直在改變。所以最好只是利用當前觀測數據來修正，不要牽扯過去。其實過去的數據也通過狀態轉移來傳遞，例如協方差。但沒必要所有都傳遞。

所以修改最小二乘法。使它只需要使用 （即最新的觀測數據）來更新修正 。

最小二乘法與卡爾曼濾波

在最小二乘法中：

得：

因為: ,其中：

得到：

整理一下：

這樣，我們不需要維護在的狀態轉移下的形式。只需要維護

在卡爾曼濾波中的形式為：

結合共同組成了卡爾曼濾波的基本形式。其實就是最小二乘法

到這裡，唯一與我們看到的卡爾曼濾波那幾個公式形式不同的是的計算。下面給出遞推計算 .

有時候，為了方便計算，會定義： .

出現了經典的五個公式的影子。

遞推計算

一個精巧的矩陣反演：

任意給定兩個可逆矩陣

令： ,計算可得：

這也就是說，只要矩陣可逆：

那麼遞推計算

根據

令：

則：

卡爾曼濾波更新公式總結

如此，卡爾曼濾波的基本形式也已經走通。本質上還是最小二乘法。只是在狀態轉移時傳遞了協方差。那麼最小二乘法的幾種形式均可以引用在卡爾曼濾波。例如設置遺忘。

關於這5個公式：

回到最初的那個問題。當我們知道時刻的。到了時刻，產生了一組觀測數據： .但由於的速度一直在改變。那麼我們要推算以現在的狀態 觀察歷史的數據 應該是什麼樣的。比如在的反向上做了加速。那麼觀察到的的加速度應該更大了。通過得出。同時，我們將協方差矩陣通過也傳遞過來。

然後通過的遞推最小二乘法計算當前中觀察下的的狀態。

如麼沒有的狀轉移，那麼我們其實也得出了遞推版本的最小二乘法

設置初始值

如何確定呢? .

根據最小二乘法：

得到：

其中，足夠小。可以定義：

即：

這是不是就在說明，當我們沒有任何觀測數據時，的方差為