基于RSSI的機(jī)器人室內(nèi)卡爾曼濾波定位算法

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

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

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

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

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

6、信息處理系統(tǒng)的思維方法和基本開發(fā)過(guò)程。4、在上學(xué)期的基礎(chǔ)上，使定位節(jié)點(diǎn)（機(jī)器人）在室內(nèi)做勻速運(yùn)動(dòng)和勻加速運(yùn)動(dòng)，實(shí)現(xiàn)拉爾曼濾波定位算法。2、 設(shè)計(jì)任務(wù)與要求及軟件介紹任務(wù)： 用RSSI方法實(shí)現(xiàn)機(jī)器人室內(nèi)定位，繼而進(jìn)行卡爾曼濾波定位算法。要求：首先根據(jù)所選題目，設(shè)計(jì)實(shí)現(xiàn)系統(tǒng)的原理框圖，并建立系統(tǒng)的模型：裝用八個(gè)測(cè)量節(jié)點(diǎn)的實(shí)驗(yàn)室（長(zhǎng)17m，寬7m）。其次用Matlab仿真二維路線圖（），產(chǎn)生模擬距離數(shù)據(jù),進(jìn)而用定位算法得到機(jī)器人位置,以及根據(jù)給定位置信息，繪制二維路線圖。再次設(shè)計(jì)卡爾曼濾波算法，用Matlab仿真卡爾曼濾波定位算法，得到機(jī)器人位置。最后寫出設(shè)計(jì)報(bào)告。軟件介紹：MATLAB是由美國(guó)ma

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

8、理與通訊、設(shè)計(jì)與分析等領(lǐng)域。 MATLAB的基本數(shù)據(jù)單位是，它的指令表達(dá)式與數(shù)學(xué)、工程中常用的形式十分相似，故用MATLAB來(lái)解算問(wèn)題要比用C，F(xiàn)ORTRAN等語(yǔ)言完成相同的事情簡(jiǎn)捷得多，并且MATLAB也吸收了像Maple等軟件的優(yōu)點(diǎn)，使MATLAB成為一個(gè)強(qiáng)大的。在新的版本中也加入了對(duì)，的支持?？梢灾苯诱{(diào)用,用戶也可以將自己編寫的實(shí)用程序?qū)氲組ATLAB函數(shù)庫(kù)中方便自己以后調(diào)用，此外許多的MATLAB愛(ài)好者都編寫了一些經(jīng)典的程序，用戶可以直接進(jìn)行下載就可以用。三、設(shè)計(jì)步驟1、實(shí)驗(yàn)預(yù)習(xí) 根據(jù)此次課程設(shè)計(jì)要求：首先，根據(jù)所選題目，設(shè)計(jì)實(shí)現(xiàn)系統(tǒng)的原理框圖，并建立系統(tǒng)的模型。其次，查閱有關(guān)Mat

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

10、考節(jié)點(diǎn)和1個(gè)定位節(jié)點(diǎn)組成的Zig-Bee 無(wú)線傳感器網(wǎng)絡(luò)。如圖2，室內(nèi)節(jié)點(diǎn)布置俯視圖，18為8個(gè)固定參考節(jié)點(diǎn)，還有一個(gè)定位節(jié)點(diǎn)。參考節(jié)點(diǎn)地址以及坐標(biāo)見表一。該定位系統(tǒng)的工作過(guò)程，先通過(guò)定位節(jié)點(diǎn)采集8個(gè)參考節(jié)點(diǎn)的信號(hào)強(qiáng)度，根據(jù)信號(hào)強(qiáng)度以及相關(guān)無(wú)線電理論求得定位節(jié)點(diǎn)與8個(gè)參考節(jié)點(diǎn)之間的距離。然后利用距離信息和參考節(jié)點(diǎn)的位置建立定位模型，從而確定定位節(jié)點(diǎn)的位置。再者建立卡爾曼濾波模型，對(duì)定位節(jié)點(diǎn)的位置信息濾波處理。表一參考節(jié)點(diǎn)設(shè)置表參考節(jié)點(diǎn)地址坐標(biāo)/參考節(jié)點(diǎn)地址坐標(biāo)/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)室內(nèi)定位模型建立本文利用各個(gè)參考節(jié)點(diǎn)采集定位節(jié)點(diǎn)的RSSI值，依據(jù)RSSI值和距離的特定關(guān)系，建立RSSI值和距離的數(shù)學(xué)模型為： （1）式中，n 為信號(hào)傳播常量；d 為距發(fā)射器的距離；A為距離1米時(shí)的RSSI值。假設(shè)參考節(jié)點(diǎn)的空間位置為，定位節(jié)點(diǎn)的空間位置為，定位節(jié)點(diǎn)和參考節(jié)點(diǎn)之間的距離為，得到方程： （2）為求解方程，至少需要4個(gè)參考節(jié)點(diǎn)，對(duì)公式（2）進(jìn)行線性化處理。將式（2）在近似位置處展開成線性方程，寫成矩陣形式為： （3）式中，；。令，則式（3）可寫成下列形式：，即 （4）當(dāng)參考節(jié)點(diǎn)多于4個(gè)時(shí)，方程

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

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

14、的預(yù)測(cè)過(guò)程： （8） （9）（2）卡爾曼濾波器方程的校正過(guò)程： （10） （11） （12）用戶位置移動(dòng)的三維模型為： （13） （14）初值的選?。?，5、設(shè)計(jì)結(jié)果勻速運(yùn)動(dòng)：圖3. 仿真機(jī)器人勻速運(yùn)動(dòng)二維路線圖 圖4. 機(jī)器人勻速運(yùn)動(dòng)x方向位置 圖5.機(jī)器人勻速運(yùn)動(dòng)y方向位置Matlab仿真產(chǎn)生的機(jī)器人室內(nèi)二維路線圖，如圖3。相應(yīng)方向、方向的位置。圖6.機(jī)器人勻速運(yùn)動(dòng)真實(shí)、觀測(cè)位置信息圖7.機(jī)器人勻速運(yùn)動(dòng)x、y觀測(cè)位置信息圖8.機(jī)器人勻速運(yùn)動(dòng)x方向真實(shí)、觀測(cè)、濾波位置信息圖9.機(jī)器人勻速運(yùn)動(dòng)y方向真實(shí)、觀測(cè)、濾波位置信息 Matlab仿真的機(jī)器人真實(shí)、觀測(cè)位置信息，如圖6。仿真產(chǎn)生的x、y方向

15、的觀測(cè)位置信息，如圖7。用Matlab仿真卡爾曼濾波算法，并用其對(duì)觀測(cè)位置信息濾波，如圖8、圖9。勻加速運(yùn)動(dòng)：圖10. 仿真機(jī)器人勻加速運(yùn)動(dòng)二維路線圖 圖11. 機(jī)器人勻加速運(yùn)動(dòng)x方向位置 圖12.機(jī)器人勻加速運(yùn)動(dòng)y方向位置圖13.機(jī)器人勻加速運(yùn)動(dòng)真實(shí)、觀測(cè)位置信息圖14.機(jī)器人勻加速運(yùn)動(dòng)x、y觀測(cè)位置信息圖15.機(jī)器人勻加速運(yùn)動(dòng)x方向真實(shí)、觀測(cè)、濾波位置信息圖16.機(jī)器人勻加速運(yùn)動(dòng)y方向真實(shí)、觀測(cè)、濾波位置信息Matlab仿真的勻加速運(yùn)動(dòng)的機(jī)器人真實(shí)、觀測(cè)位置信息，如圖13。仿真產(chǎn)生的x、y方向的觀測(cè)位置信息，如圖14。用Matlab仿真卡爾曼濾波算法，并用其對(duì)觀測(cè)位置信息濾波，如圖15、圖

16、16。 四、設(shè)計(jì)總結(jié)根據(jù)前面4-5頁(yè)的軟件介紹，我知道Matlab是一款功能齊全的數(shù)據(jù)處理軟件。首先，用Matlab仿真產(chǎn)生機(jī)器人二維位置信息（如圖3、10），為后續(xù)加噪聲、濾波等信息處理做準(zhǔn)備。產(chǎn)生方法如下：% 產(chǎn)生模擬理論位置數(shù)據(jù)% 勻速運(yùn)動(dòng)t = 0:0.1:12.9; v = 1; s0 = 1;s = s0 + v*t;x = s;y = 3*x./x;% 勻加速運(yùn)動(dòng)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;其次，根據(jù)節(jié)點(diǎn)信息將二維位置信息轉(zhuǎn)為距

17、離信息，再用距離信息求機(jī)器人位置信息，然后對(duì)機(jī)器人位置信息加噪聲（如圖6、7和圖13、14）。% 轉(zhuǎn)位置數(shù)據(jù)為距離數(shù)據(jù)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仿真卡爾曼濾波算法，并用其對(duì)加有噪聲機(jī)器人位置信息濾波（如圖8、9和圖15、16）?？柭鼮V波算法如下

18、：% 卡爾曼濾波 N = n; w = randn(1,N); %系統(tǒng)預(yù)測(cè)的隨機(jī)白噪聲w(1) = 0;xs1(1) = 0; a = 1; for k = 2:N; xs1(k) = a*xs1(k-1) + w(k-1); %系統(tǒng)的預(yù)測(cè)值end V = randn(1,N); %測(cè)量值的隨機(jī)白噪聲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; %測(cè)量值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; %前一時(shí)刻X的相關(guān)系數(shù)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); %經(jīng)過(guò)濾波后的信號(hào)p(t) = p1(t) - c*b(t)*p1(t);%t狀態(tài)下x(t|t)的相關(guān)系數(shù)end五、 實(shí)驗(yàn)心得6、 參考文獻(xiàn) 1.樊昌信等.通信原理（第三版）.北京：國(guó)防工業(yè)出版社.20042.鄭君里等.信號(hào)與系統(tǒng)（第二版）.北京：高等教育出版社.20003.安 雷、張國(guó)良、湯文俊.基于RSSI

20、的機(jī)器人室內(nèi)卡爾曼濾波定位算法研究4.彭 渤.基于RSSI測(cè)距誤差補(bǔ)償?shù)臒o(wú)線傳感器網(wǎng)絡(luò)定位算法研究5.朱登科. 基于 RSSI 的無(wú)線傳感器網(wǎng)絡(luò)測(cè)距和定位技術(shù)研究七、附錄（源代碼）% 產(chǎn)生模擬理論位置數(shù)據(jù)close all;clear all;clc;% 勻加速運(yùn)動(dòng)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('真實(shí)位置');title(&#

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

22、rid on;legend('y方向真實(shí)位置');% 轉(zhuǎn)位置數(shù)據(jù)為距離數(shù)據(jù)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 % 保存數(shù)據(jù)為.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('真實(shí)位置','觀測(cè)位置');title('二維線路圖');xlabel('長(zhǎng)：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); %系統(tǒng)預(yù)測(cè)的隨機(jī)白噪聲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方向系統(tǒng)的預(yù)測(cè)值ys1(

29、k) = a*ys1(k-1) + w(k-1); %y方向系統(tǒng)的預(yù)測(cè)值end V = randn(1,N); %測(cè)量值的隨機(jī)白噪聲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方向測(cè)量值Y2 = c.*yd + c1.*V; %y方向測(cè)量值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; %前一時(shí)刻X的相關(guān)系數(shù)p3(t) = a.2*p2(t-1) + Rww; %前一時(shí)刻Y的相關(guān)系數(shù)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方向經(jīng)過(guò)濾波后的信號(hào)s2(t)= a*s2(t-1) + b(t)*(Y2(t)-a*c*s2(t-1); %y方向經(jīng)過(guò)濾波后的信號(hào)p(t) = p1(t) - c*b(t)*p1(t);%

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

32、;y方向觀測(cè)值');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方向?yàn)V波值','x方向真實(shí)值','x方向觀測(cè)值');xlabel('采樣點(diǎn)');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àn)V波值','y方向真實(shí)值','y方向觀測(cè)值');xlabel('采樣點(diǎn)');ylabel('y方向位置/m');% 真實(shí)值、觀測(cè)值、濾波值的均值和方差% 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);% 保存數(shù)據(jù)為.matsave('s1.mat','s1');save('s2.mat','s2');% 產(chǎn)生模擬理論位置數(shù)據(jù)close all;clear all;clc;% 勻速運(yùn)動(dòng)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('真實(shí)位置');title('二維線路圖');xlabel('長(zhǎng)：x/m');ylabel('寬：y/m');figure;plot(x,'r.-');xlabel('采樣點(diǎn)');ylabel('x方向位置/m');title('x方向線路圖');grid on;l

36、egend('x方向真實(shí)位置');figure;plot(y,'k.-');xlabel('采樣點(diǎn)');ylabel('y方向位置/m');title('y方向線路圖');grid on;legend('y方向真實(shí)位置');% 轉(zhuǎn)位置數(shù)據(jù)為距離數(shù)據(jù)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 % 保存數(shù)據(jù)為.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('真實(shí)位置','觀測(cè)位置');title('二維線路圖');xlabel('長(zhǎng)：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); %系統(tǒng)預(yù)測(cè)的隨機(jī)白噪聲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方向系統(tǒng)的預(yù)測(cè)值ys1(k) = a*ys1(k-1) + w(k-1); %y方向系統(tǒng)的預(yù)測(cè)值end V = randn(1,N); %測(cè)量值的隨機(jī)白噪聲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方向測(cè)量值Y2 = c.*yd + c1.*V; %y方向測(cè)量值p(1) = 20; p2(1) = 20; s1(1) = 0;s2(1) = 0;for t = 2:N; p1(t)