20180805卡爾曼濾波說明

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

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

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

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

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

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

7、到的協(xié)方差矩陣。同理對于測量系統(tǒng)。最后兩個正太分布相乘之后化簡得到：=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ù)測得到的系統(tǒng)量;6就是系統(tǒng)最后的到的協(xié)方差矩陣,并且由于本系統(tǒng)的測量值就是系統(tǒng)狀態(tài)向量，所以H為單位向量。把u和6使用測量和預(yù)測的狀態(tài)向量代替得到如下方程組：由這三個公式在加上（1）和（3）便是整個卡爾曼濾波的過程和公式的由來。*面是計算卡爾曼的過程：這個流程中有一個問題，就是Qt和Rt如何獲得，首先對于Qt和Rt都是屬于系統(tǒng)的誤差帶進來的，和系統(tǒng)的狀態(tài)向量是無關(guān)向量，所以可以認(rèn)為Qt和Rt的兩個狀態(tài)量是線性無關(guān)的，既然是線性無關(guān)，那么Qt和Rt就是一個對角矩陣。出對角線元素以外，其他元素都為零。對于對角線上的值，同樣假設(shè)系

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

10、代表的是帶有波動的真是數(shù)據(jù)，紅色是混疊了其他干擾最后測量得到的結(jié)果?？柭A(yù)測通過預(yù)測值和紅色的觀測值，預(yù)測得到的結(jié)果為黑色線條的結(jié)果和藍色的真是結(jié)果是十分相近的。真實數(shù)據(jù)：但是這里有一個問題，就是我的Rt和Qt的協(xié)方差矩陣中的方差必須要選擇的和系統(tǒng)真實的類似。只能通過人工手動的去選擇，只有選擇到和合適的才能夠很好的預(yù)測下一次的位置，如果選擇的不合適，就會出現(xiàn)卡爾曼平滑或者是預(yù)測點不收斂的情況。/*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;%時間間隔A=1T;01;%狀態(tài)轉(zhuǎn)移矩陣B=(T*T)/2T;%控制矩陣Z=x;y;%觀測值X=x;y;%真實值H=10;01;%測量值與狀態(tài)量之間的轉(zhuǎn)換矩陣a=1;%加速度%添加噪聲；此處量參數(shù)用于仿真，實際中是由系統(tǒng)的和測量設(shè)備的精度來決定的W=1,0.5*(rand(2,N).*2-1);%由系統(tǒng)自帶的噪聲引入的噪聲量，距離為1m的誤差；速度為土0.5m的誤差V=3,1.5*(rand(2,N).*2-1);%由測量系統(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é)方差矩陣；此處在實際使用的時候，是要根據(jù)具體來調(diào)整的。只有設(shè)定的值和系統(tǒng)正式方差越接近，估計效果越好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(