淺談卡爾曼濾波
文章以最小二乘法為切入點展開討論卡爾曼濾波。卡爾曼濾波與最小二乘法只有很小的區別。
經典最小二乘法解決的問題： 考慮下面的 個測量：
每次測量都有自己的誤差. 不可觀測，
。
是適當維度的向量。最小二乘法通過最小化下面的式子來獲得對
最有可能的估計：
這產生了一個問題，那就是每次測量誤差都不一樣，不能一概而論，如果每次測量都同樣的權重。會導致結果被某些誤差比較大的數據帶偏。（其實多數情況下誤差分佈也是不知道的），所以更通用的形式是帶權最小二乘法：
其中:
考慮一個有點複雜的問題：
兩架飛機 我們想通過在
中觀察到的
位置信息，近似得到速度，加速度，等信息。
而往往我們自身的狀態也在改變。我們對自身狀態的改變也是一個帶有雜訊的測量。
這一過程可以表示為：
是不相關，均值為
的雜訊。一般記：
表示對
估計
這裡，如果我們單看 式，這其實是一個最小二乘法可以解決的問題。
式子對應觀測數據。比如第
次測量
的相對位置，也就是
則可以驗證
。理想情況下，我們估計的
會有：
.往往測量是有誤差的
。而在驗證之前，因為上一時刻
的速度，位置等狀態信息與現在有改變，導致測量的結果與
狀態的改變有關。要通過
式子對歷史
進行狀態轉移。 這相當於不同參考系觀測的數據要處理到同一個參考系。這是因為一個人看到另一個人速度的快慢也跟自身速度有關。
式是一組狀態轉移，
是
時刻，系統的一個輸入。
則是對上一時刻狀態的轉移。例如，上一時刻
觀察到
的位置信息到當前的變換。這裡
理解為參考系
狀態的改變。比如產生了一個加速。改變了你觀測
的相對加速度。
卡爾曼濾波將會以下面的方式來估計狀態：
上圖中，假設 只能得到
相對於自身的位置和兩次測量之間的時間間隔，以及自身位置速度，加速度等信息。
黃色箭頭表示我們對 的速度，加速度等狀態的估計。由於
的
時刻到與
時刻速度，加速度，位置等信息的不同，所以需要先通過狀態轉移對歷史估計轉移到新的估計。再利用
最新的位置來 矯正 轉移後新的估計。獲得
時刻
狀態的估計。
如果沒有 的狀態轉移，可以直接最小二乘法通過極小化目標函數來求
。
注意：此時可以假設 相當於靜止的。且
勻加速運動，觀測信息
是總時間的轉移矩陣而不是時間間隔。你可以認為
.這個舉例只是你便於理解上面
組成的系統，也可以不用參考。由於
的靜止，我門忽略掉
.此時就是最小二乘法可以解決的問題。
最小二乘法
其中，
其中：
對每一次觀測進行歸一化.（這裡，其實默認假設了
)
你也可以認為 表示了權重，即帶權最小二乘法（其實權重可以不僅僅限於
）
為什麼權重要是 呢。單看
式，不同時刻誤差的大小，讓我們對某些測量結果不是那麼相信。但雖然有不好的測量，但也一定程度上反映了真實的結果。經過歸一化後，每次測量誤差都變成了均值為
，方差為
。而方差相同，意味著兩次測量的可信度相同了。
這裡其實假設了我們已經知道了測量誤差的方差，但現實中我們往往是不知道的。把它假設出來，只是為了便於數學上的說明。如果不知道，那麼大可認為方差都是一樣。此時第一直覺是設置一個常數。其實大可認為是 。因為此時不管取何數值，都不會影響最終結果。因為各項權重都是一樣的。
那麼一般最小二乘法中， ,按照帶權最小二乘法來說，我們默認了
經典的求導,令：
則：
一般會記:
正如標題曲線擬合那樣，最小二乘法曲線擬合是不會隨時間變化的。而 式組曾的系統卻反應出的是一個會變的
,也就是說，
是有區別的。所以
與
兩者測量的
已經不一樣了。
下面你會看到卡爾曼濾波如何將最小二乘法應用到上面 組成的系統中。
協方差矩陣
注意： ，即為
的協方差矩陣。這是卡爾曼濾波的關鍵。
證明：
其中：
解釋一下：
當我們知道誤差 時，這其實就是確定性求解。
則：
這裡， ，
,這是因為，不同時刻的測量誤差是不相關的。
注意：到這裡我們只需要知道了協方差矩陣即可立即有： ![hat x = P_kH_k^TR_kY_k]()
進一步令
得到一組遞推關係。將協方差轉為在線計算。
協方差傳遞
注意，之所以要傳遞協方差很容易理解。上一時刻與當前時刻的狀態是不同。比如參考系 位置，速度，加速度的改變。
由於 就是協方差
的這一性質，那麼當經過
式，狀態轉移時，協方差也可以傳遞。
對於：
解釋一下，對於前面的式子中，都是在最小二乘法的背景下來討論的。那個時候， 是不隨時間變化的。而在加入
的狀態轉移後，狀態的轉移，使得
時刻的
與
時刻的
有所不同。所以要有下標來做時間上的區分。
那麼：
協方差傳遞矩陣為：
其中： .
這樣就方便我們將最小二乘法擴展到 所組成的系統。
卡爾曼濾波
表示在不參考
與
時，系統對
的估計。
表示參考了
後，系統對
的估計。
這裡，為了區分矩陣的逆。用上標 表示先驗。
但，此時 這個操作是不對的，按照
來說，因為參考系
在不斷的運動。觀察到的數據兩個時刻差異可能很大。但被觀察的
卻是一個勻加速運動。這就好比你 過去的觀察數據 和 現在的觀察數據 參考系的 狀態一直在改變。所以最好只是利用當前觀測數據來修正，不要牽扯過去。其實過去的數據也通過狀態轉移來傳遞，例如協方差。但沒必要所有都傳遞。
所以修改最小二乘法。使它只需要使用 （即最新的觀測數據）來更新修正
。
最小二乘法與卡爾曼濾波
在最小二乘法中：
得：
因為: ,其中：
得到：
整理一下：
這樣，我們不需要維護 在
的狀態轉移下的形式。只需要維護
在卡爾曼濾波中的形式為：
結合 共同組成了卡爾曼濾波的基本形式。其實就是最小二乘法
到這裡，唯一與我們看到的卡爾曼濾波那幾個公式形式不同的是 的計算。下面給出遞推計算
.
有時候，為了方便計算，會定義： .
出現了 經典的五個公式的影子。
遞推計算 ![P_k]()
一個精巧的矩陣反演：
任意給定兩個可逆矩陣
令： ,計算可得：
這也就是說，只要矩陣 可逆：
那麼遞推計算
根據
令：
則：
卡爾曼濾波更新公式總結
如此，卡爾曼濾波的基本形式也已經走通。本質上還是最小二乘法。只是在狀態轉移時傳遞了協方差。那麼最小二乘法的幾種形式均可以引用在卡爾曼濾波。例如設置遺忘。
關於這5個公式：
回到最初 的那個問題。當我們知道
時刻的
。到了
時刻，產生了一組觀測數據：
.但由於
的速度一直在改變。那麼我們要推算以現在的狀態
觀察歷史的數據
應該是什麼樣的。比如
在
的反向上做了加速。那麼觀察到的
的加速度應該更大了。通過
得出。同時，我們將協方差矩陣
通過
也傳遞過來。
然後通過 的遞推最小二乘法計算當前
中觀察下的
的狀態。
如麼沒有 的狀轉移，那麼我們其實也得出了遞推版本的最小二乘法
設置初始值
如何確定呢? .
根據最小二乘法：
得到：
其中， 足夠小。可以定義：
即：
這是不是就在說明，當我們沒有任何觀測數據時， 的方差為
推薦閱讀：
