多元信息融合報(bào)告-擴(kuò)展卡爾曼濾波

1、目錄一、仿真背景2二、仿真算法2三、仿真結(jié)果4四、總結(jié)8圖表Figure1EKF的算法結(jié)構(gòu)3Figure2真實(shí)路徑5Figure3不加EKF路徑5Figure4不加EKF誤差6Figure5加EKF路徑6Figure6加EKF誤差7Figure7真實(shí)、不加EKF、加EKF路徑對(duì)比7Figure8不加EKF、加EKF誤差對(duì)比8一、仿真背景室內(nèi)定位很難采用GPS定位的方法，由于信號(hào)受建筑物的影響而大大衰減造成的，同時(shí)由于多路徑效應(yīng)也會(huì)導(dǎo)致定位精度較低。在新主樓室內(nèi)測(cè)試過幾種GPS模塊，發(fā)現(xiàn)只要在室內(nèi)就是不能成功定位的，但只要把GPS模塊靠近空曠側(cè)的窗，GPS信號(hào)就會(huì)恢復(fù)，但是定位精度卻比較差?？梢?/p>

2、，小范圍的室內(nèi)定位用GPS定位方法是不合適的。目前室內(nèi)定位有很多其他方法，例如室內(nèi)無線定位技術(shù)、基于計(jì)算機(jī)視覺、光跟蹤定位、基于圖像分析、磁場(chǎng)以及信標(biāo)定位等。此外，還有基于圖像分析的定位技術(shù)、信標(biāo)定位、三角定位等。與GPS定位方法比較，這些技術(shù)成熟度上還存在不足，但是這些技術(shù)有很好的應(yīng)用前景，尤其是在小范圍、小空間的室內(nèi)定位中有很好的定位效果、精度也很高，目前也有很高的研究熱度。把這些技術(shù)和GPS方法結(jié)合起來，就可以發(fā)揮各自的優(yōu)長(zhǎng)，既可以提供較好的精度和響應(yīng)速度，又可以覆蓋較廣的范圍，實(shí)現(xiàn)無縫的、精確的定位，把這些不同來源的傳感器信息綜合起來，就需要依靠多源信息融合的方法實(shí)現(xiàn)。本實(shí)驗(yàn)主要是對(duì)室

3、內(nèi)定位數(shù)據(jù)進(jìn)行濾波，從而獲取更好的定位效果。實(shí)驗(yàn)數(shù)據(jù)是基于圖像分析的方法獲取的，以實(shí)驗(yàn)室天花板嵌入的燈罩作為地標(biāo)，燈罩具有確定的分布以及確定的位置，可以把這些燈罩作為參考，解算出攝像頭的位置，再把攝像頭坐標(biāo)系轉(zhuǎn)換到車坐標(biāo)系上，這樣就實(shí)現(xiàn)了定位。實(shí)驗(yàn)數(shù)據(jù)來自于老師提供的文件，文件數(shù)據(jù)導(dǎo)入到matlab中，使用matlab對(duì)數(shù)據(jù)進(jìn)行處理，并繪圖顯示出處理效果。二、仿真算法本文使用擴(kuò)展卡爾曼濾波EKF的方法，對(duì)從圖像解析出來的數(shù)據(jù)進(jìn)行處理。擴(kuò)展卡爾曼濾波的原理如下：在非線性系統(tǒng)中，系統(tǒng)模型是這樣的：xk+1=f(Xk)+wkzk=h(Xk)+vk這與線性系統(tǒng)的區(qū)別在于非線性系統(tǒng)的狀態(tài)向量和其系數(shù)是不

4、能夠分離的。所謂擴(kuò)展卡爾曼濾波器，就是適用于非線性系統(tǒng)的卡爾曼濾波器。它與經(jīng)典的線性卡爾曼濾波器很相似，算法步驟和結(jié)構(gòu)都相同。不同在于系統(tǒng)模型和矩陣A和H。在擴(kuò)展卡爾曼濾波器當(dāng)中用非線性系統(tǒng)模型方程代替線性系統(tǒng)墨香的系統(tǒng)方程；將系統(tǒng)模型求偏導(dǎo)得到新的擴(kuò)展卡爾曼濾波器當(dāng)中的矩陣A和H,在偏導(dǎo)的求解過程中，也是就是線性化的過程中用前一個(gè)時(shí)刻的估計(jì)值作為參考點(diǎn)。通過這樣的修改就得到了適用于非線性系統(tǒng)的擴(kuò)展卡爾曼濾波器。在使用的過程中我們要時(shí)刻牢記，擴(kuò)展卡爾曼濾波會(huì)發(fā)散。擴(kuò)展卡爾曼濾波的算法結(jié)構(gòu)如下圖，注意，將非線性系統(tǒng)中的f對(duì)x求(xk估計(jì))處的偏導(dǎo)得到A,同樣的求h對(duì)x求(xk)處的偏導(dǎo)得到H。；

5、14Figure1EKF的算法結(jié)構(gòu)利用擴(kuò)展卡爾曼濾波，就可以對(duì)我們的數(shù)據(jù)進(jìn)行融合估計(jì)了下面對(duì)我們的matlab程序做一些說明：1.數(shù)據(jù)存放在location.txt文件中，數(shù)據(jù)的存放格式如下：數(shù)據(jù)分別代表：0-071.5645199.2392.41.865.4946456.4132.42.43.102842.048781.54719單位時(shí)間左右輪駛過的距離特征點(diǎn)1的image坐標(biāo)特征點(diǎn)1的world坐標(biāo)特征點(diǎn)2的image坐標(biāo)特征點(diǎn)2的world坐標(biāo)真實(shí)坐標(biāo)和角度2. 程序中的變量說明：ru,rv：pixelmagnificationfactors，像素放大因子xf,yf：worldframe

6、coordinates，世界坐標(biāo)xfr,yfr：robotframecoordinates，robot坐標(biāo)uO,vO：imagecoordinatesofthecamerasprincipal機(jī)投影點(diǎn)的圖像坐標(biāo)zfc：distancefromtheopticalcenterofthecameratoceilingplane，相機(jī)中心至U天花板平面的距離d1,d2：coordinatesofOcintherobotframe,相機(jī)中心在robot坐標(biāo)系中的坐標(biāo)b：distancebetweenthedrivingwheels,兩輪距P,Q：論文中的測(cè)距法模型矩陣W：觀測(cè)模型矩陣u,v：coord

7、inatesofthefeatureintheimageplane,image坐標(biāo)中的特征位置3. 程序中的變量定義以及融合過程完全按照文章中的步驟進(jìn)行，最終將得至：a. 真實(shí)的路徑b. 不加EKF計(jì)算出的路徑c. 加EKF計(jì)算出的路徑d. 不加EKF計(jì)算和真實(shí)值之間的誤差e. 加EKF計(jì)算和真實(shí)值之間的誤差三、仿真結(jié)果根據(jù)上述數(shù)據(jù)提取以及處理的方法與流程，得至了下面的結(jié)果：1.真實(shí)路徑Figure3不加EKF路徑3.不加EKF的誤差5.加EKF的誤差Figure6加EKF誤差7.不加EKF和加EKF誤差的綜合對(duì)照四、總結(jié)通過對(duì)比我們可以看出，加EKF后融合的數(shù)據(jù)要比不加EKF的數(shù)據(jù)好的多，無

8、論是從路徑圖上還是從誤差曲線上，都有明顯的效果?？梢?，使用濾波的方法對(duì)傳感器數(shù)據(jù)進(jìn)行處理可以為我們很好地提高數(shù)據(jù)的精度，這對(duì)于我們更精確地完成一些操作是必須的。本科階段參加過一些比賽和小制作，也經(jīng)常使用一些傳感器，但總是被傳感器精度不足困擾。多源信息融合這門課給了我新的工具與思路，通過采用多個(gè)不同、或者相同的傳感器數(shù)據(jù)進(jìn)行融合，就可以很好地提高傳感器精度，獲得更好的數(shù)據(jù)。這從我們這次仿真的圖像中就可以看出來它的效果。以后再進(jìn)行一些工程或者項(xiàng)目的時(shí)候，會(huì)盡量使用信息融合的辦法，采集不同來源的數(shù)據(jù)以提高傳感器精度。這其實(shí)是對(duì)我思維的改變。另外，這門課讓我認(rèn)識(shí)到自己的數(shù)學(xué)基礎(chǔ)特別薄弱，而且面對(duì)文獻(xiàn)耐

9、心不足，這也是以后需要著力提升的方面。最后，感謝老師寬限我晚交報(bào)告附錄：matlab代碼：clearall;clc;ticdata=importdata(location.txt);ru=902.13283;rv=902.50141;u0=347.20436;v0=284.34705;zfc=2.1050;d1=-0.0668;d2=0.0536;b=0.328;I=eye(3);0025;P=0.00300;00.0030;000.01;Q_value=0.1*0.050;00.05;W=0.01*25000;02500;00250;0k0(1,1:2)=data(6,:);k0(1,3)=

10、data(7,1);d_s=zeros(2,2);route_real=zeros(2,3);dataf=zeros(2,2,2);datat=zeros(2,2,2);count=1;fori=1:size(data,1)ifmod(i,7)=3dataf(count,:,1)=data(i,:);endifmod(i,7)=5dataf(count,:,2)=data(i,:);endifmod(i,7)=6route_real(count,1:2)=data(i,:);endifmod(i,7)=0route_real(count,3)=data(i,1);count=count+1;e

11、ndifmod(i,7)=2datat(count,:,1)=data(i,:);endifmod(i,7)=4datat(count,:,2)=data(i,:);endifmod(i,7)=1d_s(count,:)=data(i,:);endendX=k0(1,:);count=2;fori=1:size(d_s,1)k0(count,1)=k0(count-1,1)+(d_s(i,1)+d_s(i,2)/2*cos(k0(count-1,3);k0(count,2)=k0(count-1,2)+(d_s(i,1)+d_s(i,2)/2*sin(k0(count-1,3);k0(coun

12、t,3)=k0(count-1,3)+(d_s(i,1)-d_s(i,2)/b;count=count+1;endroute_fusion=zeros(size(d_s,1),2);fori=1:size(d_s,1)X_estimate=X(1,1)+(d_s(i,1)+d_s(i,2)/2*cos(X(3,1);X(2,1)+(d_s(i,1)+d_s(i,2)/2*sin(X(3,1);X(3,1)+(d_s(i,1)-d_s(i,2)/b;hx=zeros(3,3);hx(1,1)=1;hx(2,2)=1;hx(3,3)=1;hx(1,3)=-(d_s(i,1)+d_s(i,2)/2*

13、sin(X(3,1);hx(2,3)=(d_s(i,1)+d_s(i,2)/2*cos(X(3,1);hu=1/b*ones(3,2);hu(3,2)=0-hu(3,2);hu(1,1)=0.5*cos(X(3,1);hu(2,1)=0.5*sin(X(3,1);hu(1,2)=0.5*cos(X(3,1);hu(2,2)=0.5*sin(X(3,1);Q=Q_value;Q(1,1)=Q(1,1)*abs(d_s(i,1);Q(2,2)=Q(2,2)*abs(d_s(i,2);P_estimate=hx*P*hx+hu*Q*hu;z_estimate=-ru/zfc*(-(dataf(i,1

14、,1)-X_estimate(1,1)*sin(X_estimate(3,1)+(dataf(i,2,1)-X_estimate(2,1)*cos(X_estimate(3,1)-d2)+u0;-rv/zfc*(-(dataf(i,1,1)-X_estimate(1,1)*cos(X_estimate(3,1)-(dataf(i,2,1) -X_estimate(2,1)*sin(X_estimate(3,1)+d1)+v0;-ru/zfc*(-(dataf(i,1,2)-X_estimate(1,1)*sin(X_estimate(3,1)+(dataf(i,2,2) -X_estimate

15、(2,1)*cos(X_estimate(3,1)-d2)+u0;-rv/zfc*(-(dataf(i,1,2)-X_estimate(1,1)*cos(X_estimate(3,1)-(dataf(i,2,2) -X_estimate(2,1)*sin(X_estimate(3,1)+d1)+v0;gx=ones(4,3);gx(1,1)=ru/zfc*sin(X(3,1);gx(1,2)=-ru/zfc*cos(X(3,1);gx(1,3)=ru/zfc*(-(dataf(i,1,1)-X(1,1)*cos(X(3,1)-(dataf(i,2,1)-X(2,1)*sin(X(3,1);gx

16、(2,1)=rv/zfc*cos(X(3,1);gx(2,2)=rv/zfc*sin(X(3,1);gx(2,3)=rv/zfc*(dataf(i,1,1)-X(1,1)*sin(X(3,1)-(dataf(i,2,1)-X(2,1)*cos(X(3,1) );gx(3,1)=ru/zfc*sin(X(3,1);gx(3,2)=-ru/zfc*cos(X(3,1);gx(3,3)=ru/zfc*(-(dataf(i,1,2)-X(1,1)*cos(X(3,1)-(dataf(i,2,2)-X(2,1)*sin(X(3,1);gx(4,1)=rv/zfc*cos(X(3,1);gx(4,2)=r

17、v/zfc*sin(X(3,1);gx(4,3)=rv/zfc*(dataf(i,1,2)-X(1,1)*sin(X(3,1)-(dataf(i,2,2)-X(2,1)*cos(X(3,1) );gx=gx*(-1);R=gx*P_estimate*(gx)+W;K=P_estimate*(gx)*inv(R);Z=zeros(4,1);Z(1,1)=datat(i,1,1);Z(2,1)=datat(i,2,1);Z(3,1)=datat(i,1,2);Z(4,1)=datat(i,2,2);X_fusion=X_estimate+K*(Z-z_estimate);X=X_fusion;P=(I-K*gx)*P_estimate;route_fusion(i,:)=X_fusion(1:2,1);endfigure;plot(route_real(:,1),route_real(:,2);holdon;plot(k0(:,1),k0(:,2),r);holdon;plot(route_fusion(:,1),route_fusion(:,2),k);holdoff;E_odometer=zeros(size(route_real,1),1);E_fusion=zeros(size(route_real,1),1);fori=1:size(route_rea