卡爾曼濾波是一種利用線性系統狀態方程，通過系統輸入輸出觀測數據，對系統狀態進行最優估計的算法。由于觀測數據中包括系統中的噪聲和干擾的影響，所以最優估計也可看作是濾波過程。上面一段話來自百度百科，其實最核心的意思就是卡爾曼濾波可以很好地從帶有噪聲的數據過程中估計狀態。而且卡爾曼濾波也是阿波羅登月中使用的突破性技術之一，正好我國嫦娥五號采土歸來，正好回顧一下卡爾曼濾波的技術細節，對公式推導做一個完整的整理。

1、定義

• 狀態方程：

• 觀測方程：

• 符號定義：

首先定義狀態估計誤差

理解卡爾曼公式推導的核心，是一定要理解這里的符號標記。核心是在第k時刻，對k+1時刻的估計！所以后面都要記住，在第k時刻的時候，對k+1時刻的任何值，都只能是估計（預測未來值）。具體怎么估計的，如下：

同理，我們也可以得到觀測估計誤差

基于上面的公式，我們定義兩個重要的誤差協方差矩陣

• 最終目的

其實卡爾曼的最終目的，是得到一個基于誤差能夠不斷修正的迭代式估計表達式，其具體形式應該如下：

這個式子非常直觀，就是基于誤差去修正，怎么最優的去修正？就是我們怎么來算這個W，也就是卡爾曼增益（Kalman Gain）。

2、推導

為了得到不停的基于誤差修正的一種計算模式，可能要能夠得到一些關鍵的遞推形式，如我之前發表過關于遞推最小二乘的推導。首先我們推導關于狀態誤差協方差矩陣：

這里我們得到了P(k|k) 到P(k+1|k)的一個遞推形式，注意這里P(k|k)的標記形式是為了更方便后面的推導，為什么這么說，其實是為了得到

這樣一種能夠不停向前迭代的形式。帶著這個思考繼續下面的推導，就會明白卡爾曼的巧妙之處。

我們先總結下，這里得到了P的一個遞推形式：

同理，也是可以得到觀測誤差協方差的一個遞推形式：

會發現其實S(k+1)和S(k)沒啥直接的關系，這就是為什么不把S(k+1)寫成像P那樣S(K+1|K)的形式，因為沒有必要，只有P是需要這樣來寫的。

這里再次思考，我們的目的是求一個最優的W，就是卡爾曼增益，具體怎么來最優？這里就用到了狀態估計誤差，就是說，我們的目的是讓狀態誤差的平方和最小，這里可以使用P的跡。具體推導如下：

然后使用P的跡對W求導，令其為0得到最優的W：

得到：

然后再把W帶回P(k+1|k+1)的式子中：

到這里，推導完畢，得到了W的最優表達式，同時，也解決了我們上面提出的問題，一個完整的遞推鏈：

建立完畢。那么通過這一套完整的遞推鏈，給定P的一個初始估計P(0|0)就可以按照下面的鏈來進行狀態估計：

到這里卡爾曼濾波的公式推導完成了，這個版本是我在看過很多其他資料，反復提煉之后，標記最友好，推導最友好的一個版本，理解卡爾曼濾波一定要首先理解符號的定義，特別是下標，以及怎么樣構建遞推鏈。在掌握之后可以自行通過上面的推導總結出，其他教程反復提到的黃金五條公式，其實這些在我看來不是重點，重點是理解其本質原理。

3、資料

其實要弄懂卡爾曼濾波還真不簡單，需要一些基本矩陣、統計、自控原理以及矩陣求導的知識，特別是對跡求導，這里推薦Matrix CookBook，里面詳細列舉了各種常用的求導方式。

總結

以上是生活随笔為你收集整理的卡尔曼_卡尔曼滤波最完整公式推导的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。