基于rssi的機(jī)器人室內(nèi)卡爾曼濾波定位算法(編輯修改稿)
2024-07-20 04:13 本頁面
【文章內(nèi)容簡介】 = a*xs1(k1) + w(k1)。 %系統(tǒng)的預(yù)測值end V = randn(1,N)。 %測量值的隨機(jī)白噪聲q1 = std(V)。 Rvv = q1.^2。 q2 = std(xs1)。 Rxx = q2.^2。q3 = std(w)。 Rww = q3.^2。 c = 1。 c1 = 。Y1 = c.*xd + c1.*V。 %測量值p(1) = 0。 p2(1) = 0。 s1(1) = 0。s2(1) = 0。for t = 2:N。 p1(t) = a.^2*p(t1) + Rww。 %前一時刻X的相關(guān)系數(shù)b(t) = c*p1(t)/(c.^2*p1(t) + Rvv)。 %卡爾曼增益s1(t) = a*s1(t1) + b1(t)*(Y1(t)a*c*s1(t1))。 %經(jīng)過濾波后的信號p(t) = p1(t) c*b(t)*p1(t)。%t狀態(tài)下x(t|t)的相關(guān)系數(shù)end五、 實(shí)驗(yàn)心得 參考文獻(xiàn) （第三版）.北京：（第二版）.北京： 雷、張國良、 . 基于 RSSI 的無線傳感器網(wǎng)絡(luò)測距和定位技術(shù)研究七、附錄（源代碼）%% 產(chǎn)生模擬理論位置數(shù)據(jù)close all。clear all。clc。% 勻加速運(yùn)動t = 0::5。s0 = 。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,39。39。)。grid on。legend(39。真實(shí)位置39。)。title(39。二維線路圖39。)。xlabel(39。長：x/m39。)。ylabel(39。寬：y/m39。)。figure。plot(x,39。39。)。xlabel(39。采樣點(diǎn)39。)。ylabel(39。x方向位置/m39。)。title(39。x方向線路圖39。)。grid on。legend(39。x方向真實(shí)位置39。)。figure。plot(y,39。39。)。xlabel(39。采樣點(diǎn)39。)。ylabel(39。y方向位置/m39。)。title(39。y方向線路圖39。)。grid on。legend(39。y方向真實(shí)位置39。)。%% 轉(zhuǎn)位置數(shù)據(jù)為距離數(shù)據(jù)Rs = [0,0,2。0,2。0,6,2。8,6,2。16,6,2。16,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 %% xs = x。save(39。39。,39。xs39。)。ys = y。save(39。39。,39。ys39。)。zs = z。save(39。39。,39。zs39。)。ds = d。save(39。39。,39。ds39。)。%% 用模擬值定位close all。clear all。clc。Rs = [0,0,2。0,2。0,6,2。8,6,2。16,6,2。16,2。16,0,2。8,0,2]。D = load(39。39。)。X = load(39。39。)。Y = load(39。39。)。n = length()。x = zeros(n,3)。zd = 0。for m = 1:n for i=1:8 d(m,i) = (m,i)。 endendfor m = 1:n xs(m) = (m)。 ys(m) = (m)。endfor m = 1:n for xd = 0::17 for yd = 0::7 if (abs((xdRs(1,1))^2+(ydRs(1,2))^2+(zdRs(1,3))^2d(m,1)^2)amp。amp。... abs((xdRs(2,1))^2+(ydRs(2,2))^2+(zdRs(2,3))^2d(m,2)^2)amp。amp。... abs((xdRs(3,1))^2+(ydRs(3,2))^2+(zdRs(3,3))^2d(m,3)^2)amp。amp。... abs((xdRs(4,1))^2+(ydRs(4,2))^2+(zdRs(4,3))^2d(m,4)^2)amp。amp。... abs((xdRs(5,1))^2+(ydRs(5,2))^2+(zdRs(5,3))^2d(m,5)^2)amp。amp。... abs((xdRs(6,1))^2+(ydRs(6,2))^2+(zdRs(6,3))^2d(m,6)^2)amp。amp。... abs((xdRs(7,1))^2+(ydRs(7,2))^2+(zdRs(7,3))^2d(m,7)^2)amp。amp。... abs((xdRs(8,1))^2+(ydRs(8,2))^2+(zdRs(8,3))^2d(m,8)^2)) 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 + .*randn(1,n)。yd = yd + .*randn(1,n)。figure。plot(xs,ys,39。39。)。hold on。plot(xd,yd,39。o39。)。grid on。legend(39。真實(shí)位置39。,39。觀測位置39。)。title(39。二維線路圖39。)。xlabel(39。長：x/m39。)。ylabel(39。寬：y/m39。)。hold off。save(39。39。,39。xd39。)。save(39。39。,39。yd39。)。%% 卡爾曼濾波 Xs = load(39。39。)。Ys = load(39。39。)。Xd = load(39。39。)。Yd = load(39。39。)。n = length()。for i1 = 1:n xs(i1) = (i1)。 ys(i1) = (i1)。 xd(i1) = (i1)。 yd(i1) = (i1)。endN = n。 w = randn(1,N)。 %系統(tǒng)預(yù)測的隨機(jī)
環(huán)評公示相關(guān)推薦
鄂ICP備17016276號-1