20180805卡爾曼濾波說(shuō)明

1、卡爾曼濾波說(shuō)明什么是卡爾曼濾波：你可以在任何含有不確定因素的動(dòng)態(tài)系統(tǒng)里使用卡爾曼濾波，而且你應(yīng)該可以通過(guò)某種數(shù)學(xué)建模對(duì)系統(tǒng)下一步動(dòng)向做一個(gè)大概的預(yù)測(cè)。盡管系統(tǒng)總是會(huì)受到一些未知的干擾，但是卡爾曼濾波總是可以派上用場(chǎng)來(lái)提高系統(tǒng)預(yù)估的精確度，這樣你就可以更加準(zhǔn)確地知道到底發(fā)生了什么事情（系統(tǒng)狀態(tài)是如何轉(zhuǎn)移的）。卡爾曼是使用已有的測(cè)量數(shù)據(jù)和觀測(cè)數(shù)據(jù)，再結(jié)合兩者這件的協(xié)方差等參數(shù)估計(jì)系統(tǒng)下一步狀態(tài)的一種計(jì)算方法。原理理解和過(guò)程推導(dǎo)：既然卡爾曼是結(jié)合觀測(cè)數(shù)據(jù)和預(yù)測(cè)數(shù)據(jù)來(lái)確定當(dāng)前目標(biāo)的最有位置，那么首先需要做的，就是對(duì)系統(tǒng)進(jìn)行建模。假設(shè)系統(tǒng)的狀態(tài)空間中包括了距離和速度兩項(xiàng)。即x=rV;系統(tǒng)以勻加速運(yùn)動(dòng)，系

2、統(tǒng)加速度a=1;根據(jù)牛頓運(yùn)動(dòng)定律可預(yù)測(cè)目標(biāo)的當(dāng)前位置為：）/2結(jié)合上述兩方程使用矩陣乘法形式表示如下所示：（1）令Ft=；Bt=;其中Ft稱(chēng)之為狀態(tài)狀態(tài)轉(zhuǎn)換矩陣；Bt稱(chēng)之為控制矩陣。Ft和Bt矩陣的作用是通過(guò)上一次的位置，將目標(biāo)計(jì)算到下一次位置。如下圖所示：由于在xk-1的位置上距離和速度是存在相關(guān)性的，應(yīng)為隨著速度的變化，距離也是會(huì)隨之變化的。假設(shè)在位置的協(xié)方差矩陣為（協(xié)方差矩陣表征的是兩個(gè)系統(tǒng)量的相關(guān)程度），那么經(jīng)過(guò)Ft矩陣的線性變換后到達(dá)后的協(xié)方差矩陣是多少呢？根據(jù)對(duì)稱(chēng)矩陣的性質(zhì)可以知道，假設(shè)：則;所以系統(tǒng)從位置變換到位置后，系統(tǒng)的協(xié)方差矩陣也由變?yōu)?。其中?）綜上表達(dá)式1和表達(dá)式2就已

3、經(jīng)完成了目標(biāo)的預(yù)測(cè)的過(guò)程。當(dāng)時(shí)上述過(guò)程還少了一個(gè)誤差量。這個(gè)誤差量有可能是系統(tǒng)本身自帶的或者是計(jì)算機(jī)的浮點(diǎn)誤差或是截位誤差等等。通俗來(lái)講就是說(shuō)，理論上會(huì)被映射到的位置，但是由于存在系統(tǒng)的誤差（或稱(chēng)之為轉(zhuǎn)換誤（3）差），系統(tǒng)會(huì)把這個(gè)點(diǎn)映射到真實(shí)的的附近的一個(gè)范圍內(nèi)。如下圖所示：這個(gè)誤差我們使用來(lái)表示，它實(shí)際上就可以理解為方差。其實(shí)它就是方差（測(cè)量值離均值的平方的均值）。通常是由系統(tǒng)決定的。此處需要區(qū)分兩個(gè)概念，一個(gè)是，它表征的是協(xié)方差矩陣，是兩個(gè)狀態(tài)量之間的相關(guān)程度，說(shuō)白了也是方差。如圖中藍(lán)色和紫色的斑塊。則是由系統(tǒng)帶來(lái)的誤差，為了方便計(jì)算（個(gè)人理解）也是用方差表示。所以疊加上系統(tǒng)誤差后的位置的

4、協(xié)方差矩陣表示為：*知上面一部分是介紹系統(tǒng)的預(yù)測(cè)值。并且得到了系統(tǒng)預(yù)測(cè)值得協(xié)方差矩陣，下面一部分就是關(guān)系系統(tǒng)的測(cè)量值：一般情況下我們通過(guò)傳感器就能夠直接測(cè)量得到我們的狀態(tài)空間中的變量。比如雷達(dá)能夠直接測(cè)量得到距離和速度，而距離和速度就是系統(tǒng)狀態(tài)空間中的變量。但是也不排除傳感器測(cè)量的量并不是我們狀態(tài)系統(tǒng)中的變量。比如說(shuō)，雷達(dá)傳感器無(wú)法直接測(cè)量出前方目標(biāo)的加速度，只能通過(guò)測(cè)量得到的速度和時(shí)間間隔通過(guò)公式計(jì)算出加速度。那么這個(gè)計(jì)算也可以通過(guò)矩陣來(lái)建模，這個(gè)變換矩陣稱(chēng)之為Ht矩陣。如下圖所示：對(duì)于測(cè)量值而言：=Hk*同樣經(jīng)過(guò)Hk變換后系統(tǒng)的測(cè)量的協(xié)方差矩陣也發(fā)送了變化，即但是由于測(cè)量傳感器也是存在誤差

5、的，比如雷達(dá)傳感器就會(huì)存在干擾以及信噪比不夠?qū)е碌臏y(cè)量不準(zhǔn)確等等，我們統(tǒng)稱(chēng)測(cè)量上引入的干擾也是用方差表示，稱(chēng)之為：Rt,如下圖所示：所以測(cè)量系統(tǒng)最后得到的協(xié)方差矩陣為：其中Rt與Pm的關(guān)系與Qt和Pt的關(guān)系式一樣的，注意區(qū)別。/*/到目前為止，我們已經(jīng)獲得了兩套目標(biāo)信息，一套是通過(guò)預(yù)測(cè)得到的目標(biāo)狀態(tài)，以及他的協(xié)方差矩陣；另外一套就是通過(guò)實(shí)際測(cè)量得到的一套目標(biāo)狀態(tài)信息和他的協(xié)方差矩陣?，F(xiàn)在的關(guān)鍵問(wèn)題是如何取舍，相信誰(shuí)更多一點(diǎn)。這個(gè)比例怎么分配。如下所示：圖中有兩個(gè)區(qū)域位置，一個(gè)是通過(guò)預(yù)測(cè)得到，預(yù)測(cè)得到位置為黃色正太分布的期望，但是由于存在誤差，所以他還有一個(gè)方差存在，即Pt另外一個(gè)藍(lán)色的區(qū)域，是

6、測(cè)量得到的，同樣，測(cè)量得到的狀態(tài)量帶變了藍(lán)色分布的期望值，他的協(xié)方差矩陣表征了由于誤差到來(lái)的變動(dòng)范圍。如何在這兩個(gè)分布中獲取最優(yōu)的占比。方法是獲取他們的交集。然而對(duì)于一維的正態(tài)分布，獲取他們交集的一個(gè)辦法就是把兩個(gè)正太分布的pdf（概率密度函數(shù)）相乘即可。如下圖所示：.旳求解得到的新的正太分布的期望值就是當(dāng)前幀的狀態(tài)預(yù)測(cè)值。得到的方差矩陣就會(huì)是預(yù)測(cè)后目標(biāo)可能存在的一個(gè)變動(dòng)范圍。下面介紹兩個(gè)正太分布如何相乘。在介紹之前，先把幾個(gè)變量的含義搞清楚。如上圖。首先假定紫紅色的正太分布圖代表的是預(yù)測(cè)值得；綠色代表的是傳感器測(cè)量值的。那么相對(duì)于上一時(shí)刻而言，期望值uO就是預(yù)測(cè)得到的系統(tǒng)量；6就是系統(tǒng)最后的

7、到的協(xié)方差矩陣。同理對(duì)于測(cè)量系統(tǒng)。最后兩個(gè)正太分布相乘之后化簡(jiǎn)得到：=P/l，-iH7(H,P/(/_iH7+Rr)1theKalmanain.SubstitutingH=1/candK=(/7cti)/+r$ultsinfustd=/ii+K-)r(17)ItisnoweasytoseehowthestandardKalmanfilterquatiin$reliteto(17)and418)derivedabove:帀黑&）仏-伽U8)Similarlythefusedvarianceestimatebecome一備If=+Kf(xJ=CONCLUSIONSTheKalmanfiJterca

8、nbEtaughtusinJa最后總結(jié)公式如下：666666uO就是預(yù)測(cè)得到的系統(tǒng)量;6就是系統(tǒng)最后的到的協(xié)方差矩陣,并且由于本系統(tǒng)的測(cè)量值就是系統(tǒng)狀態(tài)向量，所以H為單位向量。把u和6使用測(cè)量和預(yù)測(cè)的狀態(tài)向量代替得到如下方程組：由這三個(gè)公式在加上（1）和（3）便是整個(gè)卡爾曼濾波的過(guò)程和公式的由來(lái)。*面是計(jì)算卡爾曼的過(guò)程：這個(gè)流程中有一個(gè)問(wèn)題，就是Qt和Rt如何獲得，首先對(duì)于Qt和Rt都是屬于系統(tǒng)的誤差帶進(jìn)來(lái)的，和系統(tǒng)的狀態(tài)向量是無(wú)關(guān)向量，所以可以認(rèn)為Qt和Rt的兩個(gè)狀態(tài)量是線性無(wú)關(guān)的，既然是線性無(wú)關(guān)，那么Qt和Rt就是一個(gè)對(duì)角矩陣。出對(duì)角線元素以外，其他元素都為零。對(duì)于對(duì)角線上的值，同樣假設(shè)系

9、統(tǒng)的狀態(tài)量為距離和速度，那么對(duì)于Qt而言，是屬于系統(tǒng)的預(yù)測(cè)誤差，這個(gè)誤差主要來(lái)源是計(jì)算誤差，理論上應(yīng)該是較小的。但是其真實(shí)值我們是不知道的，所以可以選擇對(duì)于但是Rt則是測(cè)量誤差引起的，從雷達(dá)傳感器的經(jīng)驗(yàn)上看，他的誤差距離在3m左右，速度的誤差在1.5m/s左右，所以用這個(gè)值帶入得到Rt矩陣。*仿真和實(shí)際數(shù)據(jù)效果：120100305020100200300real-3lueobserY-valuee&timatfrvalue-20L02015IIr5050-10015025030tJ-5L0200real-valueobserve-valueeEtimste-/alije從上面兩幅圖中分析，藍(lán)色

10、代表的是帶有波動(dòng)的真是數(shù)據(jù)，紅色是混疊了其他干擾最后測(cè)量得到的結(jié)果?？柭A(yù)測(cè)通過(guò)預(yù)測(cè)值和紅色的觀測(cè)值，預(yù)測(cè)得到的結(jié)果為黑色線條的結(jié)果和藍(lán)色的真是結(jié)果是十分相近的。真實(shí)數(shù)據(jù)：但是這里有一個(gè)問(wèn)題，就是我的Rt和Qt的協(xié)方差矩陣中的方差必須要選擇的和系統(tǒng)真實(shí)的類(lèi)似。只能通過(guò)人工手動(dòng)的去選擇，只有選擇到和合適的才能夠很好的預(yù)測(cè)下一次的位置，如果選擇的不合適，就會(huì)出現(xiàn)卡爾曼平滑或者是預(yù)測(cè)點(diǎn)不收斂的情況。/*closeall;clearvars;clc;dbstopiferror;N=300;x=zeros(1,N);%快拍數(shù)%狀態(tài)量1（距離）y=zeros(1,N);%狀態(tài)量2（速度）x(1)=0;%

11、初始值y(1)=0;%初始值T=0.05;%時(shí)間間隔A=1T;01;%狀態(tài)轉(zhuǎn)移矩陣B=(T*T)/2T;%控制矩陣Z=x;y;%觀測(cè)值X=x;y;%真實(shí)值H=10;01;%測(cè)量值與狀態(tài)量之間的轉(zhuǎn)換矩陣a=1;%加速度%添加噪聲；此處量參數(shù)用于仿真，實(shí)際中是由系統(tǒng)的和測(cè)量設(shè)備的精度來(lái)決定的W=1,0.5*(rand(2,N).*2-1);%由系統(tǒng)自帶的噪聲引入的噪聲量，距離為1m的誤差；速度為土0.5m的誤差V=3,1.5*(rand(2,N).*2-1);%由測(cè)量系統(tǒng)引入的噪聲量（干擾、能量等），距離為3m的誤差；速度為1.5m的誤差%產(chǎn)生原始數(shù)據(jù)fork=2:NX(:,k)=A*X(:,k-

12、1)+B*a;endX=X+W;Z=X+V;%添加噪聲%添加噪聲%獲取狀態(tài)轉(zhuǎn)移初始化協(xié)方差矩陣和觀察誤差協(xié)方差矩陣；此處在實(shí)際使用的時(shí)候，是要根據(jù)具體來(lái)調(diào)整的。只有設(shè)定的值和系統(tǒng)正式方差越接近，估計(jì)效果越好Q=0.3A2*eye(2);%狀態(tài)轉(zhuǎn)移初始化協(xié)方差矩陣R=2.5A2*eye(2);%觀察誤差協(xié)方差矩陣P=1*eye(2);%初始協(xié)方差矩陣x(1)=Z(1,1);y(1)=Z(1,2);Xk=x;y;fork=2:N;Xk(:,k)=A*Xk(:,k-1)+B*a;P=A*P*A+Q;Kg=P*H/(H*P*H+R);Xk(:,k)=Xk(:,k)+Kg*(Z(:,k)-H*Xk(:,k);P=P-Kg*H*P;endfigureplot(X(1,:);holdon;plot(Z(1,:),-r);holdon;plot(Xk(