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

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

3、己預(yù)測的不確定度是4度，他們平方相加再開方，就是5）。 從溫度計(jì)那里得到了k 時(shí)刻的溫度值，假設(shè)是25度，同時(shí)該值的偏差是4度。,.,由于我們用于估算k時(shí)刻的實(shí)際溫度有兩個(gè)溫度值，分別是23度和25度。究竟實(shí)際溫度是多少呢？相信自己還是相信溫度計(jì)呢？究竟相信誰多一點(diǎn)，我們可以用他們的協(xié)方差來判斷。 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ì)的值。,.,現(xiàn)在我們已經(jīng)得到k時(shí)刻的最優(yōu)溫度值了，下一步就 是要進(jìn)入k+1時(shí)

4、刻，進(jìn)行新的最優(yōu)估算。 在進(jìn)入k+1時(shí)刻之前，我們還要算出k 時(shí)刻那個(gè)最優(yōu)值（24.56度）的偏差。算法如下：(1-Kg)*52) 0.5=2.35。這里的5就是上面的k時(shí)刻你預(yù)測的那個(gè)23度溫度值的偏差，得出的2.35就是進(jìn)入k+1時(shí)刻以后k 時(shí)刻估算出的最優(yōu)溫度值的偏差（對(duì)應(yīng)于上面的3）。,.,1. 首先我們要利用系統(tǒng)的過程模型，來預(yù)測下一狀態(tài)的系統(tǒng)。假設(shè)現(xiàn)在的系統(tǒng)狀態(tài)是k ，根據(jù)系統(tǒng)的模型，可以基于系統(tǒng)的上一狀態(tài)而預(yù)測出現(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ù)測的結(jié)果，X(k-1|k-1)是上一狀態(tài)

5、最優(yōu)的結(jié)果，U(k)為現(xiàn)在狀態(tài)的控制量，如果沒有控制量，它可以為0。,經(jīng)典卡爾曼濾波,.,經(jīng)典卡爾曼濾波,2. 到現(xiàn)在為止，我們的系統(tǒng)結(jié)果已經(jīng)更新了，可是，對(duì)應(yīng)于X(k|k-1) 的covariance還沒更新。我們用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)過程激勵(lì)噪聲協(xié)方差。式子1，2就是卡爾曼濾波器 5個(gè)公式當(dāng)中的前兩個(gè)，也就是對(duì)系統(tǒng)的預(yù)測。,.,經(jīng)典卡爾曼濾波,現(xiàn)

6、在我們有了現(xiàn)在狀態(tài)的預(yù)測結(jié)果，然后我們?cè)偈占F(xiàn) 在狀態(tài)的測量值。結(jié)合預(yù)測值和測量值，我們可以得到 現(xiàn)在狀態(tài)(k) 的最優(yōu)化估算值X(k|k)： X(k|k)= X(k|k-1)+Kg(k) (Z(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),.,經(jīng)典卡爾曼濾波,到現(xiàn)在為止，我們已經(jīng)得到了k 狀態(tài)下最優(yōu)的估算值X(k|k)。但是為了要另卡爾曼濾波器不斷的運(yùn)行下去直到系統(tǒng)過程結(jié)束，我們還要更新k狀態(tài)下X(k|k)的covariance： P(k|k)=（I-Kg(k) H）P(k|k-1) (5) 其中I 為1的矩陣，對(duì)于單模型單測量，I=1。當(dāng)系統(tǒng)進(jìn)入k+1狀態(tài)時(shí)，P(k|k)就是