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

1、定義

• 狀態(tài)方程：

• 觀測方程：

• 符號定義：

首先定義狀態(tài)估計(jì)誤差

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

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

基于上面的公式，我們定義兩個(gè)重要的誤差協(xié)方差矩陣

• 最終目的

其實(shí)卡爾曼的最終目的，是得到一個(gè)基于誤差能夠不斷修正的迭代式估計(jì)表達(dá)式，其具體形式應(yīng)該如下：

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

2、推導(dǎo)

為了得到不停的基于誤差修正的一種計(jì)算模式，可能要能夠得到一些關(guān)鍵的遞推形式，如我之前發(fā)表過關(guān)于遞推最小二乘的推導(dǎo)。首先我們推導(dǎo)關(guān)于狀態(tài)誤差協(xié)方差矩陣：

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

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

我們先總結(jié)下，這里得到了P的一個(gè)遞推形式：

同理，也是可以得到觀測誤差協(xié)方差的一個(gè)遞推形式：

會發(fā)現(xiàn)其實(shí)S(k+1) 和S(k) 沒啥直接的關(guān)系，這就是為什么不把S(k+1) 寫成像P那樣 S(K+1|K) 的形式，因?yàn)闆]有必要，只有P是需要這樣來寫的。

這里再次思考，我們的目的是求一個(gè)最優(yōu)的W，就是卡爾曼增益，具體怎么來最優(yōu)？這里就用到了狀態(tài)估計(jì)誤差，就是說，我們的目的是讓狀態(tài)誤差的平方和最小，這里可以使用P的跡。具體推導(dǎo)如下：

然后使用P的跡對W求導(dǎo)，令其為0得到最優(yōu)的W：

得到：

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

到這里，推導(dǎo)完畢，得到了W的最優(yōu)表達(dá)式，同時(shí)，也解決了我們上面提出的問題，一個(gè)完整的遞推鏈：

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

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

3、資料

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