基于rssi的機(jī)器人室內(nèi)卡爾曼濾波定位算法(文件)
【正文】 量值Y2 = c.*yd + c1.*V。s2(1) = 0。 %前一時(shí)刻Y的相關(guān)系數(shù)b(t) = c*p1(t)/(c.^2*p1(t) + Rvv)。 %y方向經(jīng)過(guò)濾波后的信號(hào)p(t) = p1(t) c*b(t)*p1(t)。39。plot(ys1,39。title(39。plot(Y1,39。hold on。hold off。legend(39。)。)。r*39。plot(xd,39。title(39。x方向?yàn)V波值39。x方向觀測(cè)值39。)。figure(5)。grid on。)。ko39。y方向定位效果39。,39。)。ylabel(39。xs_var = var(xs)。s1_var = var(s1)。yd_var = var(yd)。39。save(39。)。% 勻速運(yùn)動(dòng)t = 0::。x = s。plot(x,y,39。legend(39。二維線路圖39。)。figure。xlabel(39。x方向位置/m39。)。)。)。ylabel(39。y方向線路圖39。y方向真實(shí)位置39。0,6,2。16,0,2。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)。,39。save(39。)。,39。save(39。)。Rs = [0,0,2。16,6,2。D = load(39。39。)。for m = 1:n for i=1:8 d(m,i) = (m,i)。amp。amp。amp。amp。 else end end endendfor m = 1:n xd(m) = x(m,1)。figure。hold on。grid on。觀測(cè)位置39。)。ylabel(39。save(39。)。yd39。)。Xd = load(39。39。 ys(i1) = (i1)。 w = randn(1,N)。a = 1。 %y方向系統(tǒng)的預(yù)測(cè)值end V = randn(1,N)。 Rxx = q2.^2。 Rww = q3.^2。 %x方向測(cè)量值Y2 = c.*yd + c1.*V。s2(1) = 0。 %前一時(shí)刻Y的相關(guān)系數(shù)b(t) = c*p1(t)/(c.^2*p1(t) + Rvv)。 %y方向經(jīng)過(guò)濾波后的信號(hào)p(t) = p1(t) c*b(t)*p1(t)。39。plot(ys1,39。title(39。plot(Y1,39。hold on。hold off。legend(39。)。)。r*39。plot(xd,39。title(39。x方向?yàn)V波值39。x方向觀測(cè)值39。)。figure(5)。grid on。)。ko39。y方向定位效果39。,39。)。ylabel(39。xs_var = var(xs)。s1_var = var(s1)。yd_var = var(yd)。39。save(39。)。,39。s139。s2_var = var(s2)。ys_var = var(ys)。xd_var = var(xd)。)。采樣點(diǎn)39。,39。legend(39。hold off。hold on。plot(ys,39。39。x方向位置/m39。xlabel(39。x方向真實(shí)值39。)。)。hold off。hold on。plot(s1,39。,39。測(cè)量值39。39。)。)。)。grid on。%t狀態(tài)下y(t|t)的相關(guān)系數(shù)end figure(2)。%y方向卡爾曼增益s1(t) = a*s1(t1) + b1(t)*(Y1(t)a*c*s1(t1))。 p1(t) = a.^2*p(t1) + Rww。 p2(1) = 20。 c1 = 。Ryy = q22.^2。 Rvv = q1.^2。 xs1(k) = a*xs1(k1) + w(k1)。xs1(1) = 0。 yd(i1) = (i1)。n = length()。)。39。%% 卡爾曼濾波 Xs = load(39。39。,39。)。長(zhǎng)：x/m39。title(39。真實(shí)位置39。o39。39。endxd = xd + .*randn(1,n)。 x(m,2) = yd。amp。amp。amp。 ys(m) = (m)。x = zeros(n,3)。Y = load(39。)。16,0,2。0,6,2。clear all。,39。)。save(39。,39。)。save(39。n = length(x)。16,6,2。%% 轉(zhuǎn)位置數(shù)據(jù)為距離數(shù)據(jù)Rs = [0,0,2。grid on。)。采樣點(diǎn)39。plot(y,39。legend(39。title(39。)。39。寬：y/m39。xlabel(39。)。)。z = 0.*x。s0 = 1。clear all。,39。s139。s2_var = var(s2)。ys_var = var(ys)。xd_var = var(xd)。)。采樣點(diǎn)39。,39。legend(39。hold off。hold on。plot(ys,39。39。x方向位置/m39。xlabel(39。x方向真實(shí)值39。)。)。hold off。hold on。plot(s1,39。,39。測(cè)量值39。39。)。)。)。grid on。%t狀態(tài)下y(t|t)的相關(guān)系數(shù)end figure(2)。%y方向卡爾曼增益s1(t) = a*s1(t1) + b1(t)*(Y1(t)a*c*s1(t1))。 p1(t) = a.^2*p(t1) + Rww。 p2(1) = 20。 c1 = 。Ryy = q22.^2。 Rvv = q1.^2。 xs1(k) = a*xs1(k1) + w(k1)。xs1(1) = 0。 yd(i1) = (i1)。n = length()。)。39。%% 卡爾曼濾波 Xs = load(39。39。,39。)。長(zhǎng)：x/m39。title(39。真實(shí)位置39。o39。39。endxd = xd + .*randn(1,n)。 x(m,2) = yd。amp。amp。amp。 ys(m) = (m)。x = zeros(n,3)。Y = load(39。)。16,0,2。0,6,2。clear all。,39。)。save(39。,39。)。save(39。n = length(x)。16,6,2。%% 轉(zhuǎn)位置數(shù)據(jù)為距離數(shù)據(jù)Rs = [0,0,2。grid on。)。采樣點(diǎn)39。plot(y,39。legend(39。title(39。)。39。寬：y
環(huán)評(píng)公示相關(guān)推薦
鄂ICP備17016276號(hào)-1