擴(kuò)展卡爾曼濾波原理及其應(yīng)用

1、擴(kuò)展卡爾曼濾波原理及其應(yīng)用u假設(shè)當(dāng)前室內(nèi)溫度僅跟上一時(shí)刻有關(guān)u溫度計(jì)觀測(cè)（攝氏華氏）u根據(jù)連續(xù)的觀測(cè)值來(lái)推算實(shí)際溫度變化u假設(shè)當(dāng)前室內(nèi)溫度僅跟上一時(shí)刻有關(guān) 但變化中可能有誤差u溫度計(jì)觀測(cè)（攝氏華氏） 讀數(shù)可能有誤差u兩種噪聲互不相關(guān)u根據(jù)連續(xù)的觀測(cè)值來(lái)推算實(shí)際溫度變化根據(jù)你的經(jīng)驗(yàn)判斷，這個(gè)房間的溫度是恒定的，也就是下一分鐘的溫度等于現(xiàn)在這一分鐘的溫度（假設(shè)我們用一分鐘來(lái)做時(shí)間單位）。假設(shè)你對(duì)你的經(jīng)驗(yàn)不是100%的相信，可能會(huì)有上下偏差幾度。我們把這些偏差看成是高斯白噪聲（White Gaussian Noise），也就是這些偏差跟前后時(shí)間是沒(méi)有關(guān)系的而且符合高斯分配（Gaussian Dist

2、ribution）。我們?cè)诜块g里放一個(gè)溫度計(jì)，但是這個(gè)溫度計(jì)也不準(zhǔn)確的，測(cè)量值會(huì)比實(shí)際值偏差。我們也把這些偏差看成是高斯白噪聲。對(duì)于某一分鐘我們有兩個(gè)有關(guān)于該房間的溫度值：根據(jù)經(jīng)驗(yàn)的預(yù)測(cè)值（系統(tǒng)的預(yù)測(cè)值）和溫度計(jì)的值（測(cè)量值）。假如我們要估算k 時(shí)刻的是實(shí)際溫度值。首先要根據(jù)k-1 時(shí)刻的溫度值，來(lái)預(yù)測(cè)k 時(shí)刻的溫度。因?yàn)橄嘈艤囟仁呛愣ǖ?，所以得到k 時(shí)刻的溫度預(yù)測(cè)值是跟k-1 時(shí)刻一樣的，假設(shè)是23度，同時(shí)該值的高斯噪聲的偏差是5度（5是這樣得到的：如果k-1 時(shí)刻估算出的最優(yōu)溫度值的偏差是3，你對(duì)自己預(yù)測(cè)的不確定度是4度，他們平方相加再開方，就是5）。從溫度計(jì)那里得到了k 時(shí)刻的溫度值，假

3、設(shè)是25度，同時(shí)該值的偏差是4度。由于我們用于估算k時(shí)刻的實(shí)際溫度有兩個(gè)溫度值，分別是23度和25度。究竟實(shí)際溫度是多少呢？相信自己還是相信溫度計(jì)呢？究竟相信誰(shuí)多一點(diǎn)，我們可以用他們的協(xié)方差來(lái)判斷。Kg2=52/(52+42)，所以Kg=0.78 ，我們可以估算出k 時(shí)刻的實(shí)際溫度值是：23+0.78* (25-23)=24.56度?？梢钥闯?，因?yàn)闇囟扔?jì)的協(xié)方差比較?。ū容^相信溫度計(jì)），所以估算出的最優(yōu)溫度值偏向溫度計(jì)的值?，F(xiàn)在我們已經(jīng)得到k時(shí)刻的最優(yōu)溫度值了，下一步就是要進(jìn)入k+1時(shí)刻，進(jìn)行新的最優(yōu)估算。在進(jìn)入k+1時(shí)刻之前，我們還要算出k 時(shí)刻那個(gè)最優(yōu)值（24.56度）的偏差。算法如下：(

4、1-Kg)*52) 0.5=2.35。這里的5就是上面的k時(shí)刻你預(yù)測(cè)的那個(gè)23度溫度值的偏差，得出的2.35就是進(jìn)入k+1時(shí)刻以后k 時(shí)刻估算出的最優(yōu)溫度值的偏差（對(duì)應(yīng)于上面的3）。1. 首先我們要利用系統(tǒng)的過(guò)程模型，來(lái)預(yù)測(cè)下一狀態(tài)的系統(tǒng)。假設(shè)現(xiàn)在的系統(tǒng)狀態(tài)是k ，根據(jù)系統(tǒng)的模型，可以基于系統(tǒng)的上一狀態(tài)而預(yù)測(cè)出現(xiàn)在狀態(tài)：X(k|k-1)=A X(k-1|k-1)+B U(k) . (1)式(1) 中，X(k|k-1) 是利用上一狀態(tài)預(yù)測(cè)的結(jié)果，X(k-1|k-1)是上一狀態(tài)最優(yōu)的結(jié)果，U(k)為現(xiàn)在狀態(tài)的控制量，如果沒(méi)有控制量，它可以為0。2. 到現(xiàn)在為止，我們的系統(tǒng)結(jié)果已經(jīng)更新了，可是，對(duì)應(yīng)

5、于X(k|k-1) 的covariance還沒(méi)更新。我們用P表示covariance：P(k|k-1)=A P(k-1|k-1) A+Q (2)式(2) 中，P(k|k-1) 是X(k|k-1) 對(duì)應(yīng)的covariance，P(k-1|k-1)是X(k-1|k-1)對(duì)應(yīng)的covariance，A表示A的轉(zhuǎn)置矩陣，Q是系統(tǒng)過(guò)程激勵(lì)噪聲協(xié)方差。式子1，2就是卡爾曼濾波器5個(gè)公式當(dāng)中的前兩個(gè)，也就是對(duì)系統(tǒng)的預(yù)測(cè)?，F(xiàn)在我們有了現(xiàn)在狀態(tài)的預(yù)測(cè)結(jié)果，然后我們?cè)偈占F(xiàn)在狀態(tài)的測(cè)量值。結(jié)合預(yù)測(cè)值和測(cè)量值，我們可以得到現(xiàn)在狀態(tài)(k) 的最優(yōu)化估算值X(k|k)：X(k|k)= X(k|k-1)+Kg(k) (Z

6、(k)-H X(k|k-1) (3)其中Kg為卡爾曼增益(Kalman Gain)：Kg(k)= P(k|k-1) H / (H P(k|k-1) H + R) (4)到現(xiàn)在為止，我們已經(jīng)得到了k 狀態(tài)下最優(yōu)的估算值X(k|k)。但是為了要另卡爾曼濾波器不斷的運(yùn)行下去直到系統(tǒng)過(guò)程結(jié)束，我們還要更新k狀態(tài)下X(k|k)的covariance：P(k|k)=（I-Kg(k) H）P(k|k-1) (5)其中I 為1的矩陣，對(duì)于單模型單測(cè)量，I=1。當(dāng)系統(tǒng)進(jìn)入k+1狀態(tài)時(shí)，P(k|k)就是式子(2) 的P(k-1|k-1)。這樣，算法就可以自回歸的運(yùn)算下去。1/1()()kk kkfxFxh xHx1_1_1_1_1(cossin )(sincos )kkxkkkykxkxkxkykykxkkkxxvtyyvtvvaaytvvaaytt_100001000010(sincos )0001(co