kalman filter 推導(dǎo)

1、KalmanFilter(卡爾曼濾波卡爾曼濾波)的推導(dǎo)的推導(dǎo)基本動(dòng)態(tài)系統(tǒng)模型基本動(dòng)態(tài)系統(tǒng)模型卡爾曼濾波建立在線性代數(shù)和隱馬爾可夫模型(hiddenMarkovmodel)上。其基本動(dòng)態(tài)系統(tǒng)可以用一個(gè)馬爾可夫鏈表示，該馬爾可夫鏈建立在一個(gè)被高斯噪聲(即正態(tài)分布的噪聲)干擾的線性算子上的。系統(tǒng)的狀態(tài)可以用一個(gè)元素為實(shí)數(shù)的向量表示。隨著離散時(shí)間的每一個(gè)增加這個(gè)線性算子就會(huì)作用在當(dāng)前狀態(tài)上，產(chǎn)生一個(gè)新的狀態(tài)并也會(huì)帶入一些噪聲同時(shí)系統(tǒng)的一些已知的

2、控制器的控制信息也會(huì)被加入。同時(shí)，另一個(gè)受噪聲干擾的線性算子產(chǎn)生出這些隱含狀態(tài)的可見(jiàn)輸出。為了從一系列有噪聲的觀察數(shù)據(jù)中用卡爾曼濾波器估計(jì)出被觀察過(guò)程的內(nèi)部狀態(tài)，我們必須把這個(gè)過(guò)程在卡爾曼濾波的框架下建立模型。也就是說(shuō)對(duì)于每一步k，定義矩陣FkHkQkRk，有時(shí)也需要定義Bk，如下。卡爾曼濾波器的模型。圓圈代表向量，方塊代表矩陣，星號(hào)代表高斯噪聲，其協(xié)方差矩陣在右下方標(biāo)出?？柭鼮V波模型假設(shè)k時(shí)刻的真實(shí)狀態(tài)是從(k?1)時(shí)刻的狀態(tài)演化而

3、來(lái)，符合下式：其中?Fk是作用在xk?1上的狀態(tài)變換模型（矩陣矢量）。?Bk是作用在控制器向量uk上的輸入－控制模型。?wk是過(guò)程噪聲，并假定其符合均值為零，協(xié)方差矩陣為Qk的多元正態(tài)分布。時(shí)刻k，對(duì)真實(shí)狀態(tài)xk的一個(gè)測(cè)量zk滿(mǎn)足下式:其中Hk是觀測(cè)模型它把真實(shí)狀態(tài)空間映射成觀測(cè)空間，vk是觀測(cè)噪聲，其均值為零，協(xié)方差矩陣為Rk且服從正態(tài)分布。初始狀態(tài)以及每一時(shí)刻的噪聲x0w1...wkv1...vk都認(rèn)為是互相獨(dú)立的.實(shí)際上，很多真實(shí)

4、世界的動(dòng)態(tài)系統(tǒng)都并不確切的符合這個(gè)模型；但是由于卡爾曼濾波器被設(shè)計(jì)在有噪聲的情況下工作一個(gè)近似的符合已經(jīng)可以使這個(gè)濾波器非常有用了。更多其它更復(fù)雜的卡爾曼濾波器的變種，在下邊討論中有描述?？柭鼮V波器卡爾曼濾波器卡爾曼濾波是一種遞歸的估計(jì)，即只要獲知上一時(shí)刻狀態(tài)的估計(jì)值以及當(dāng)前狀態(tài)的觀測(cè)值就可以計(jì)算出當(dāng)前狀態(tài)的估計(jì)值，因此不需要記錄觀測(cè)或者估計(jì)的歷史信息。卡爾曼濾波器與大多數(shù)濾波器不同之處，在于它是一種純粹的時(shí)域?yàn)V波器，它不需要像低通濾

5、波器等推導(dǎo)后驗(yàn)協(xié)方差矩陣推導(dǎo)后驗(yàn)協(xié)方差矩陣按照上邊的定義，我們從誤差協(xié)方差開(kāi)始推導(dǎo)如下：代入再代入與整理誤差向量，得因?yàn)闇y(cè)量誤差vk與其他項(xiàng)是非相關(guān)的，因此有利用協(xié)方差矩陣的性質(zhì)，此式可以寫(xiě)作使用不變量Pk|k1以及Rk的定義這一項(xiàng)可以寫(xiě)作：這一公式對(duì)于任何卡爾曼增益Kk都成立。如果Kk是最優(yōu)卡爾曼增益，則可以進(jìn)一步簡(jiǎn)化，請(qǐng)見(jiàn)下文。最優(yōu)卡爾曼增益的推導(dǎo)最優(yōu)卡爾曼增益的推導(dǎo)卡爾曼濾波器是一個(gè)最小均方誤差估計(jì)器，后驗(yàn)狀態(tài)誤差估計(jì)(英文:ap

6、osteriistateestimate)是我們最小化這個(gè)矢量幅度平方的期望值，，這等同于最小化后驗(yàn)估計(jì)協(xié)方差矩陣Pk|k的跡(trace)。將上面方程中的項(xiàng)展開(kāi)、抵消，得到：當(dāng)矩陣導(dǎo)數(shù)是0的時(shí)候得到Pk|k的跡(trace)的最小值：此處須用到一個(gè)常用的式子如下：從這個(gè)方程解出卡爾曼增益Kk：這個(gè)增益稱(chēng)為最優(yōu)卡爾曼增益，在使用時(shí)得到最小均方誤差。后驗(yàn)誤差協(xié)方差公式的化簡(jiǎn)后驗(yàn)誤差協(xié)方差公式的化簡(jiǎn)在卡爾曼增益等于上面導(dǎo)出的最優(yōu)值時(shí)，計(jì)算后