1、精選優質文檔-傾情為你奉上中北大學 課程設計說明書 2012-2013 學年第 一 學期學 院：信息與通信工程學院專 業：信息對抗技術專業學 生 姓 名：韓瀟灑學 號：課程設計題目：信息處理實踐基于RSSI的機器人室內卡爾曼濾波定位算法起 迄 日 期:8月20日 8月26日課程設計地點:校內指 導 教 師:楊光、王鑒、李凱、王玉、姚金杰系主任：趙冬娥下達任務書日期: 2012年8月 20 日中北大學課程設計任務書 2012/2013 學年第 一 學期  學 院： 信息與通信工程學院專 業： 信息對抗技術專業學 生 姓 名：韓瀟灑學 號： 課程設

2、計題目：信息處理實踐基于RSSI的機器人室內卡爾曼濾波定位算法起 迄 日 期:8月20日 8月26日課程設計地點: 校內指 導 教 師:楊光、王鑒、李凱、王玉、姚金杰系主任： 趙冬娥   下達任務書日期: 2012年 8 月 20 日專心-專注-專業課 程 設 計 任 務 書1設計目的：1、通過本課程設計的學習，學生將復習所學的專業知識，使課堂學習的理論知識應用于實踐，通過本課程設計的實踐使學生具有一定的實踐操作能力；2、掌握Matlab使用方法，能熟練運用該軟件設計并完成相應的信息處理；3、通過信息處理實踐的課程設計，掌握設計信息處理系統的思

3、維方法和基本開發過程。2設計內容和要求（包括原始數據、技術參數、條件、設計要求等）：一、學習Matlab軟件1、掌握Matlab軟件的安裝；2、熟悉Matlab的運行環境；3、根據所設計系統的需要會合理設定系統定時窗口的各項參數；4、根據所設計系統的需要，優化模塊設計最優系統。二、實踐設計要求：1、根據所選題目，設計實現系統的原理框圖；2、建立系統的模型；3、每人一組，寫出設計報告；4、根據給定位置信息，繪制二維路線圖；5、設計卡爾曼濾波算法；6、仿真卡爾曼濾波定位算法。三、參考題目基于RSSI的機器人室內卡爾曼濾波定位算法課 程 設 計 任 務 書3設計工作任務及工作量的要求包括課程設計計算

4、說明書(論文)、圖紙、實物樣品等：每個同學獨立完成自己的任務，每人寫一份設計報告，在課程設計論文中寫明自己設計的部分，給出設計結果。4主要參考文獻：1. 樊昌信等.通信原理（第三版）.北京：國防工業出版社.20042. 鄭君里等.信號與系統（第二版）.北京：高等教育出版社.20003. 安 雷、張國良、湯文俊.基于RSSI的機器人室內卡爾曼濾波定位算法研究5設計成果形式及要求： 設計的原理框圖，仿真模型。 課程設計說明書  6工作計劃及進度：8月20日 8月21日8月21日 8月22日8月22 日 8月24日8月24日 8月26日8月26日相關資料查閱；相關軟件的

5、學習；方案設計，完成并實現信息處理；書寫論文；答辯、成績考核。系主任審查意見：  簽字： 年 月 日目 錄一、 設計目的 4二、 設計任務與要求及軟件介紹 4三、 設計步驟 51、 實驗預習 52、 系統原理圖以及系統模型 63、 設計結果 10四、 設計總結 16五、 實驗心得 18六、 參考文獻 18七、 附錄（源代碼）181、 設計目的1、通過本課程設計的學習，學生將復習所學的專業知識，使課堂學習的理論知識應用于實踐，通過本課程設計的實踐使學生具有一定的實踐操作能力；2、掌握Matlab使用方法，能熟練運用該軟件設計并完成相應的信息處理；3、通過信息處理實踐的課程設計，掌握設計

6、信息處理系統的思維方法和基本開發過程。4、在上學期的基礎上，使定位節點（機器人）在室內做勻速運動和勻加速運動，實現拉爾曼濾波定位算法。2、 設計任務與要求及軟件介紹任務： 用RSSI方法實現機器人室內定位，繼而進行卡爾曼濾波定位算法。要求：首先根據所選題目，設計實現系統的原理框圖，并建立系統的模型：裝用八個測量節點的實驗室（長17m，寬7m）。其次用Matlab仿真二維路線圖（），產生模擬距離數據,進而用定位算法得到機器人位置,以及根據給定位置信息，繪制二維路線圖。再次設計卡爾曼濾波算法，用Matlab仿真卡爾曼濾波定位算法，得到機器人位置。最后寫出設計報告。軟件介紹：MATLAB是由美國ma

7、thworks公司發布的主要面對科學計算、可視化以及交互式程序設計的高科技計算環境。它將數值分析、科學數據可視化以及非動態系統的和仿真等諸多強大功能集成在一個易于使用的視窗環境中，為科學研究、工程設計以及必須進行有效數值計算的眾多科學提供了一種全面的解決方案，并在很大程度上擺脫了傳統非交互式程序設計語言（如C、Fortran）的編輯模式，代表了當今國際科學計算軟件的先進水平。MATLAB和、并稱為三大軟件。它在數學類科技應用軟件中在方面首屈一指。MATLAB可以進行矩陣運算、繪制和數據、實現、創建用戶界面、連 matlab開發工作界面接其他編程語言的程序等，主要應用于工程計算、控制設計、信號處

8、理與通訊、設計與分析等領域。 MATLAB的基本數據單位是，它的指令表達式與數學、工程中常用的形式十分相似，故用MATLAB來解算問題要比用C，FORTRAN等語言完成相同的事情簡捷得多，并且MATLAB也吸收了像Maple等軟件的優點，使MATLAB成為一個強大的。在新的版本中也加入了對，的支持。可以直接調用,用戶也可以將自己編寫的實用程序導入到MATLAB函數庫中方便自己以后調用，此外許多的MATLAB愛好者都編寫了一些經典的程序，用戶可以直接進行下載就可以用。三、設計步驟1、實驗預習 根據此次課程設計要求：首先，根據所選題目，設計實現系統的原理框圖，并建立系統的模型。其次，查閱有關Mat

9、lab的相關書籍、文章以及網絡資源。熟悉Matlab軟件的使用。其次，根據系統原理圖和系統模型，用Matlab仿真產生機器人二維路線圖并獲得仿真機器人位置數據，繼而與節點結合產生距離數據，在用定位算法求得機器人位置數據，并加入隨機序列白噪聲。最后，設計卡爾曼濾波算法。并用Matlab仿真卡爾曼濾波定位算法，對加有噪聲進行濾波。寫出設計報告；3、系統原理框圖以及系統模型根據系統節點和距離信息，用定位模型求解位置信息根據系統節點位置，獲得距離信息Matlab 仿真產生二維路徑，即二維位置信息隨機序列噪聲通過卡爾曼濾波器，對位置信息處理獲得加有噪聲的位置信息圖1. 系統原理框圖 該定位系統是由8個參

10、考節點和1個定位節點組成的Zig-Bee 無線傳感器網絡。如圖2，室內節點布置俯視圖，18為8個固定參考節點，還有一個定位節點。參考節點地址以及坐標見表一。該定位系統的工作過程，先通過定位節點采集8個參考節點的信號強度，根據信號強度以及相關無線電理論求得定位節點與8個參考節點之間的距離。然后利用距離信息和參考節點的位置建立定位模型，從而確定定位節點的位置。再者建立卡爾曼濾波模型，對定位節點的位置信息濾波處理。表一參考節點設置表參考節點地址坐標/參考節點地址坐標/0X0001(0,0,2)0X0101(16,6,2)0X0010(0,3.5,2)0X0110(16,3.5,2)0X0011(0,

11、6,2)0X0111(16,0,2)0X0100(8,6,2)0X1000(8,0,2)室內定位模型建立本文利用各個參考節點采集定位節點的RSSI值，依據RSSI值和距離的特定關系，建立RSSI值和距離的數學模型為： （1）式中，n 為信號傳播常量；d 為距發射器的距離；A為距離1米時的RSSI值。假設參考節點的空間位置為，定位節點的空間位置為，定位節點和參考節點之間的距離為，得到方程： （2）為求解方程，至少需要4個參考節點，對公式（2）進行線性化處理。將式（2）在近似位置處展開成線性方程，寫成矩陣形式為： （3）式中，；。令，則式（3）可寫成下列形式：，即 （4）當參考節點多于4個時，方程

12、（4）為矛盾方程。另一個計算方法為： （5）上述等式的求解可以采用最小二乘法（Least SquaresMethod，LSM）來進行迭代計算，即先從定位節點位置和傳輸距離誤差的大概值開始，然后逐步精確到計算結果滿足測量要求并將此值作為定位的最終結果。該方法求解的優點是在利用計算機求解時，可以盡可能多地利用各種有價值的信息，減小求解過程中引入的誤差。卡爾曼濾波模型的建立基于RSSI定位的重點是距離的測量，而RSSI測距容易受到干擾噪聲的影響，上述方法已不能滿足機器人對定位精度的要求。卡爾曼濾波是高斯過程最優濾波的一種有效算法，當對象模型足夠準確且系統狀態和參數不發生突變時，性能較好。因此，將卡爾

13、曼濾波與上述方法相結合，利用卡爾曼濾波器的濾波功能減小噪聲對系統的影響，達到提高定位精度的目的。首先是根據上述的定位信息建立系統的位移和速度的狀態方程并進行離散化。定位系統狀態方程為： （6） （7）式中，狀態向量為待優化的機器人定位信息，和分別為 k 時刻機器人在坐標系中三個方向的位移和速度估計值；A為系統矩陣；觀測向量S(k)為觀測的機器人定位信息，為 k 時刻機器人在坐標系中三個方向的位移的觀測值；C 為輸出矩陣；和分別為狀態噪聲和觀測噪聲，且滿足，即W(k)和V(k)是相互獨立的零均值白噪聲序列。狀態向量的初始值 X(0) 的統計特性給定為：卡爾曼濾波器計算分為：（1）卡爾曼濾波器方程

14、的預測過程： （8） （9）（2）卡爾曼濾波器方程的校正過程： （10） （11） （12）用戶位置移動的三維模型為： （13） （14）初值的選取：，5、設計結果勻速運動：圖3. 仿真機器人勻速運動二維路線圖 圖4. 機器人勻速運動x方向位置 圖5.機器人勻速運動y方向位置Matlab仿真產生的機器人室內二維路線圖，如圖3。相應方向、方向的位置。圖6.機器人勻速運動真實、觀測位置信息圖7.機器人勻速運動x、y觀測位置信息圖8.機器人勻速運動x方向真實、觀測、濾波位置信息圖9.機器人勻速運動y方向真實、觀測、濾波位置信息 Matlab仿真的機器人真實、觀測位置信息，如圖6。仿真產生的x、y方向

15、的觀測位置信息，如圖7。用Matlab仿真卡爾曼濾波算法，并用其對觀測位置信息濾波，如圖8、圖9。勻加速運動：圖10. 仿真機器人勻加速運動二維路線圖 圖11. 機器人勻加速運動x方向位置 圖12.機器人勻加速運動y方向位置圖13.機器人勻加速運動真實、觀測位置信息圖14.機器人勻加速運動x、y觀測位置信息圖15.機器人勻加速運動x方向真實、觀測、濾波位置信息圖16.機器人勻加速運動y方向真實、觀測、濾波位置信息Matlab仿真的勻加速運動的機器人真實、觀測位置信息，如圖13。仿真產生的x、y方向的觀測位置信息，如圖14。用Matlab仿真卡爾曼濾波算法，并用其對觀測位置信息濾波，如圖15、圖

16、16。 四、設計總結根據前面4-5頁的軟件介紹，我知道Matlab是一款功能齊全的數據處理軟件。首先，用Matlab仿真產生機器人二維位置信息（如圖3、10），為后續加噪聲、濾波等信息處理做準備。產生方法如下：% 產生模擬理論位置數據% 勻速運動t = 0:0.1:12.9; v = 1; s0 = 1;s = s0 + v*t;x = s;y = 3*x./x;% 勻加速運動t = 0:0.1:5; s0 = 0.1; v0 = 0; a = 1;s = s0 + v0*t +1/2.*a*t.2;x = sqrt(3)/2.*s;y = 1/2.*s;其次，根據節點信息將二維位置信息轉為距

17、離信息，再用距離信息求機器人位置信息，然后對機器人位置信息加噪聲（如圖6、7和圖13、14）。% 轉位置數據為距離數據Rs = 0,0,2;0,3.5,2;0,6,2;8,6,2;16,6,2;16,3.5,2;16,0,2;8,0,2;n = length(x);d = zeros(n,8);for m = 1:nfor i = 1:8 d(m,i) = sqrt(x(m)-Rs(i,1)2 + (y(m)-Rs(i,2)2 +(z(m)-Rs(i,3)2);endend最后，用Matlab仿真卡爾曼濾波算法，并用其對加有噪聲機器人位置信息濾波（如圖8、9和圖15、16）。卡爾曼濾波算法如下

18、：% 卡爾曼濾波 N = n; w = randn(1,N); %系統預測的隨機白噪聲w(1) = 0;xs1(1) = 0; a = 1; for k = 2:N; xs1(k) = a*xs1(k-1) + w(k-1); %系統的預測值end V = randn(1,N); %測量值的隨機白噪聲q1 = std(V); Rvv = q1.2; q2 = std(xs1); Rxx = q2.2;q3 = std(w); Rww = q3.2; c = 1; c1 = 0.1;Y1 = c.*xd + c1.*V; %測量值p(1) = 0; p2(1) = 0; s1(1) = 0;s2

19、(1) = 0;for t = 2:N; p1(t) = a.2*p(t-1) + Rww; %前一時刻X的相關系數b(t) = c*p1(t)/(c.2*p1(t) + Rvv); %卡爾曼增益s1(t) = a*s1(t-1) + b1(t)*(Y1(t)-a*c*s1(t-1); %經過濾波后的信號p(t) = p1(t) - c*b(t)*p1(t);%t狀態下x(t|t)的相關系數end五、 實驗心得6、 參考文獻 1.樊昌信等.通信原理（第三版）.北京：國防工業出版社.20042.鄭君里等.信號與系統（第二版）.北京：高等教育出版社.20003.安 雷、張國良、湯文俊.基于RSSI

20、的機器人室內卡爾曼濾波定位算法研究4.彭 渤.基于RSSI測距誤差補償的無線傳感器網絡定位算法研究5.朱登科. 基于 RSSI 的無線傳感器網絡測距和定位技術研究七、附錄（源代碼）% 產生模擬理論位置數據close all;clear all;clc;% 勻加速運動t = 0:0.1:5;s0 = 0.1;v0 = 0;a = 1;s = s0 + v0*t +1/2.*a*t.2;x = sqrt(3)/2.*s;y = 1/2.*s;z = 0.*x;figure;plot(x,y,'r.-');grid on;legend('真實位置');title(&#

21、39;二維線路圖');xlabel('長：x/m');ylabel('寬：y/m');figure;plot(x,'r.-');xlabel('采樣點');ylabel('x方向位置/m');title('x方向線路圖');grid on;legend('x方向真實位置');figure;plot(y,'k.-');xlabel('采樣點');ylabel('y方向位置/m');title('y方向線路圖');g

22、rid on;legend('y方向真實位置');% 轉位置數據為距離數據Rs = 0,0,2;0,3.5,2;0,6,2;8,6,2;16,6,2;16,3.5,2;16,0,2;8,0,2;n = length(x);d = zeros(n,8);for m = 1:n for i = 1:8 d(m,i) = sqrt(x(m)-Rs(i,1)2 + (y(m)-Rs(i,2)2 + (z(m)-Rs(i,3)2); endend % 保存數據為.matxs = x;save('xs.mat','xs');ys = y;save('

23、ys.mat','ys');zs = z;save('zs.mat','zs');ds = d;save('ds.mat','ds');% 用模擬值定位close all;clear all;clc;Rs = 0,0,2;0,3.5,2;0,6,2;8,6,2;16,6,2;16,3.5,2;16,0,2;8,0,2;D = load('ds.mat');X = load('xs.mat');Y = load('ys.mat');n = length(D.ds

24、);x = zeros(n,3);zd = 0;for m = 1:n for i=1:8 d(m,i) = D.ds(m,i); endendfor m = 1:n xs(m) = X.xs(m); ys(m) = Y.ys(m);endfor m = 1:n for xd = 0:0.001:17 for yd = 0:0.001:7 if (abs(xd-Rs(1,1)2+(yd-Rs(1,2)2+(zd-Rs(1,3)2-d(m,1)2)<0.1&&. abs(xd-Rs(2,1)2+(yd-Rs(2,2)2+(zd-Rs(2,3)2-d(m,2)2)<0.

25、1&&. abs(xd-Rs(3,1)2+(yd-Rs(3,2)2+(zd-Rs(3,3)2-d(m,3)2)<0.1&&. abs(xd-Rs(4,1)2+(yd-Rs(4,2)2+(zd-Rs(4,3)2-d(m,4)2)<0.1&&. abs(xd-Rs(5,1)2+(yd-Rs(5,2)2+(zd-Rs(5,3)2-d(m,5)2)<0.1&&. abs(xd-Rs(6,1)2+(yd-Rs(6,2)2+(zd-Rs(6,3)2-d(m,6)2)<0.1&&. abs(xd-Rs(7

26、,1)2+(yd-Rs(7,2)2+(zd-Rs(7,3)2-d(m,7)2)<0.1&&. abs(xd-Rs(8,1)2+(yd-Rs(8,2)2+(zd-Rs(8,3)2-d(m,8)2)<0.1) x(m,1) = xd; x(m,2) = yd; x(m,3) = zd; else end end endendfor m = 1:n xd(m) = x(m,1); yd(m) = x(m,2);endxd = xd + 0.2.*randn(1,n);yd = yd + 0.2.*randn(1,n);figure;plot(xs,ys,'r.-&

27、#39;);hold on;plot(xd,yd,'o-');grid on;legend('真實位置','觀測位置');title('二維線路圖');xlabel('長：x/m');ylabel('寬：y/m');hold off;save('xd.mat','xd');save('yd.mat','yd');% 卡爾曼濾波 Xs = load('xs.mat');Ys = load('ys.mat')

28、;Xd = load('xd.mat');Yd = load('yd.mat');n = length(Xs.xs);for i1 = 1:n xs(i1) = Xs.xs(i1); ys(i1) = Ys.ys(i1); xd(i1) = Xd.xd(i1); yd(i1) = Yd.yd(i1);endN = n; w = randn(1,N); %系統預測的隨機白噪聲w(1) = 0;xs1(1) = 0; ys1(1) = 0;a = 1; for k = 2:N; xs1(k) = a*xs1(k-1) + w(k-1); %x方向系統的預測值ys1(

29、k) = a*ys1(k-1) + w(k-1); %y方向系統的預測值end V = randn(1,N); %測量值的隨機白噪聲q1 = std(V); Rvv = q1.2; q2 = std(xs1); Rxx = q2.2;q22 = std(ys1);Ryy = q22.2;q3 = std(w); Rww = q3.2; c = 1; c1 = 0.05;Y1 = c.*xd + c1.*V; %x方向測量值Y2 = c.*yd + c1.*V; %y方向測量值p(1) = 20; p2(1) = 20; s1(1) = 0;s2(1) = 0;for t = 2:N; p1(t

30、) = a.2*p(t-1) + Rww; %前一時刻X的相關系數p3(t) = a.2*p2(t-1) + Rww; %前一時刻Y的相關系數b(t) = c*p1(t)/(c.2*p1(t) + Rvv); %x方向卡爾曼增益b1(t) = c*p1(t)/(c.2*p1(t) + Rvv);%y方向卡爾曼增益s1(t) = a*s1(t-1) + b1(t)*(Y1(t)-a*c*s1(t-1); %x方向經過濾波后的信號s2(t)= a*s2(t-1) + b(t)*(Y2(t)-a*c*s2(t-1); %y方向經過濾波后的信號p(t) = p1(t) - c*b(t)*p1(t);%

31、t狀態下x(t|t)的相關系數p2(t) = p3(t) - c*b(t)*p3(t);%t狀態下y(t|t)的相關系數end figure(2);plot(xs1,'r.-');grid on;hold on;plot(ys1,'b-');hold off;title('系統的預測值');figure(3);plot(Y1,'k.-');grid on;hold on;plot(Y2,'r.-');hold off;title('測量值');legend('x方向觀測值','

32、;y方向觀測值');figure(4);plot(s1,'k.-');grid on;hold on;plot(xs,'r*-');hold off;hold on;plot(xd,'bo-');hold off;title('x方向定位效果');legend('x方向濾波值','x方向真實值','x方向觀測值');xlabel('采樣點');ylabel('x方向位置/m');figure(5);plot(s2,'r.-');

33、grid on;hold on;plot(ys,'b*-');hold off;hold on;plot(yd,'ko-');hold off;title('y方向定位效果');legend('y方向濾波值','y方向真實值','y方向觀測值');xlabel('采樣點');ylabel('y方向位置/m');% 真實值、觀測值、濾波值的均值和方差% x方向xs_mean = mean(xs);xs_var = var(xs);xd_mean = mean(xd);x

34、d_var = var(xd);s1_mean = mean(s1);s1_var = var(s1);% y方向ys_mean = mean(ys);ys_var = var(ys);yd_mean = mean(yd);yd_var = var(yd);s2_mean = mean(s2);s2_var = var(s2);% 保存數據為.matsave('s1.mat','s1');save('s2.mat','s2');% 產生模擬理論位置數據close all;clear all;clc;% 勻速運動t = 0:0.1:

35、12.9;v = 1;s0 = 1;s = s0 + v*t;x = s;y = 3*x./x;z = 0.*x;figure;plot(x,y,'r.-');grid on;legend('真實位置');title('二維線路圖');xlabel('長：x/m');ylabel('寬：y/m');figure;plot(x,'r.-');xlabel('采樣點');ylabel('x方向位置/m');title('x方向線路圖');grid on;l

36、egend('x方向真實位置');figure;plot(y,'k.-');xlabel('采樣點');ylabel('y方向位置/m');title('y方向線路圖');grid on;legend('y方向真實位置');% 轉位置數據為距離數據Rs = 0,0,2;0,3.5,2;0,6,2;8,6,2;16,6,2;16,3.5,2;16,0,2;8,0,2;n = length(x);d = zeros(n,8);for m = 1:n for i = 1:8 d(m,i) = sqrt(x

37、(m)-Rs(i,1)2 + (y(m)-Rs(i,2)2 + (z(m)-Rs(i,3)2); endend % 保存數據為.matxs = x;save('xs.mat','xs');ys = y;save('ys.mat','ys');zs = z;save('zs.mat','zs');ds = d;save('ds.mat','ds');% 用模擬值定位close all;clear all;clc;Rs = 0,0,2;0,3.5,2;0,6,2;8,6,

38、2;16,6,2;16,3.5,2;16,0,2;8,0,2;D = load('ds.mat');X = load('xs.mat');Y = load('ys.mat');n = length(D.ds);x = zeros(n,3);zd = 0;for m = 1:n for i=1:8 d(m,i) = D.ds(m,i); endendfor m = 1:n xs(m) = X.xs(m); ys(m) = Y.ys(m);endfor m = 1:n for xd = 0:0.01:17 for yd = 0:0.01:7 if (

39、abs(xd-Rs(1,1)2+(yd-Rs(1,2)2+(zd-Rs(1,3)2-d(m,1)2)<0.1&&. abs(xd-Rs(2,1)2+(yd-Rs(2,2)2+(zd-Rs(2,3)2-d(m,2)2)<0.1&&. abs(xd-Rs(3,1)2+(yd-Rs(3,2)2+(zd-Rs(3,3)2-d(m,3)2)<0.1&&. abs(xd-Rs(4,1)2+(yd-Rs(4,2)2+(zd-Rs(4,3)2-d(m,4)2)<0.1&&. abs(xd-Rs(5,1)2+(yd-Rs(5

40、,2)2+(zd-Rs(5,3)2-d(m,5)2)<0.1&&. abs(xd-Rs(6,1)2+(yd-Rs(6,2)2+(zd-Rs(6,3)2-d(m,6)2)<0.1&&. abs(xd-Rs(7,1)2+(yd-Rs(7,2)2+(zd-Rs(7,3)2-d(m,7)2)<0.1&&. abs(xd-Rs(8,1)2+(yd-Rs(8,2)2+(zd-Rs(8,3)2-d(m,8)2)<0.1) x(m,1) = xd; x(m,2) = yd; x(m,3) = zd; else end end endend

41、for m = 1:n xd(m) = x(m,1); yd(m) = x(m,2);endxd = xd + 0.2.*randn(1,n);yd = yd + 0.2.*randn(1,n);figure;plot(xs,ys,'r.-');hold on;plot(xd,yd,'o-');grid on;legend('真實位置','觀測位置');title('二維線路圖');xlabel('長：x/m');ylabel('寬：y/m');hold off;save('

42、xd.mat','xd');save('yd.mat','yd');% 卡爾曼濾波 Xs = load('xs.mat');Ys = load('ys.mat');Xd = load('xd.mat');Yd = load('yd.mat');n = length(Xs.xs);for i1 = 1:n xs(i1) = Xs.xs(i1); ys(i1) = Ys.ys(i1); xd(i1) = Xd.xd(i1); yd(i1) = Yd.yd(i1);endN = n

43、; w = randn(1,N); %系統預測的隨機白噪聲w(1) = 0;xs1(1) = 0; ys1(1) = 0;a = 1; for k = 2:N; xs1(k) = a*xs1(k-1) + w(k-1); %x方向系統的預測值ys1(k) = a*ys1(k-1) + w(k-1); %y方向系統的預測值end V = randn(1,N); %測量值的隨機白噪聲q1 = std(V); Rvv = q1.2; q2 = std(xs1); Rxx = q2.2;q22 = std(ys1);Ryy = q22.2;q3 = std(w); Rww = q3.2; c = 1; c1 = 0.05;Y1 = c.*xd + c1.*V; %x方向測量值Y2 = c.*yd + c1.*V; %y方向測量值p(1) = 20; p2(1) = 20; s1(1) = 0;s2(1) = 0;for t = 2:N; p1(t)