%reader1 (-70 70) tag_1 (5 65) reader2 (-40 0) tag2(-35 5) reader3(40 0)
%tag3(35 5) reader4(40 70) 被追踪的目标tag5(tag_x,tag_y) 范围-35
clc
clear all
reader_x=[0 10 0 10];% 信号接收端横坐标
reader_y=[0 0 10 10];%纵坐标
tag_x=[0 0 0 0 0 0 0 0 0 0];%参考点坐标 %%%%%%%%%%%%%%%%%%修改tracked tags 第四个中tag_x tag_y值
tag_y=[0 0 0 0 0 0 0 0 0 0];
for i=1:10
tag_x(i)=rand*10;
tag_y(i)=rand*10;
end
delt(4,2)=1;
n=3;
P(4,2)=1;
PL1(4,2)=1;
PL2(4,2)=1;
PL3(4,2)=1;
PL4(4,2)=1;
PL5(4,2)=1;
PL6(4,2)=1;
PL7(4,2)=1;
PL8(4,2)=1;
PL9(4,2)=1;
PL10(4,2)=1;
matrix(4,3,20)=1; %定义三维数据组 matrix(a,b,:) reader_a接收到reader_b的信号窗口中的随机变量
matrix1(4,10,20)=1;
dist(4,3)=1; %定义距离数组
dist1(4,10)=1; %定义距离数组
readerx(3,3)=1;
readery(3,3)=1;
refx(4,1)=1;
refy(4,1)=1;
mean_rssi(4,3)=1; %每一个窗口的平均值
mean_rssi1(4,10)=1; %每一个窗口的平均值wu
Pl_d0=43; %PLd0为近地参考距离的路径损耗
Pt=4; %传输功率
r=1; %迭代次数为3
e(1,10)=1;
while(1)
if(r>3)
break;
end
for i=1:4
for j=1:10
dist1(i,j)=sqrt((tag_x(j)-reader_x(i))^2+(tag_y(j)-reader_y(i))^2);
matrix1(i,j,:)=Pt-Pl_d0-10*n*log10(dist1(i,j))-random('norm',0,6,1,20);
mean_rssi1(i,j)=mean(matrix1(i,j,:));
dist1(i,j)=10.^((Pt-Pl_d0-mean_rssi1(i,j))./(10*n));
end
end
for i=1:4
if ( i==1)
u=2;
m=3;
o=4;
end
if ( i==2)
u=3;
m=4;
o=1;
end
if (i==3)
u=4;
m=1;
o=2;
end
if (i==4)
u=1;
m=2;
o=3;
end
readerx(i,:)=[reader_x(u) reader_x(m) reader_x(o)];
readery(i,:)=[reader_y(u) reader_y(m) reader_y(o)];
refx(i,1)=reader_x(i);
refy(i,1)=reader_y(i);
A=[readerx(i,1),readery(i,1)];
B=[readerx(i,2),readery(i,2)];
C=[readerx(i,3),readery(i,3)];
D=[refx(i,1),refy(i,1)];
for j=1:3
dist(i,j)=sqrt((refx(i,1)-readerx(i,j))^2+(refy(i,1)-readery(i,j))^2);
matrix(i,j,:)=Pt-Pl_d0-10*n*log10(dist(i,j))-random('norm',0,6,1,20);
mean_rssi(i,j)=mean(matrix(i,j,:));
dist(i,j)=10.^((Pt-Pl_d0-mean_rssi(i,j))./(10*n));
end
Pi=Triangle(A,B,C,D,dist(i,1),dist(i,2),dist(i,3));
P(i,1)=Pi(1);
P(i,2)=Pi(2);
delt(i,1)=reader_x(i)-P(i,1);
delt(i,2)=reader_y(i)-P(i,2);
P1=Triangle2(A,B,C,tag_x(1),tag_y(1));
PL1(i,1)=P1(1)+delt(i,1);
PL1(i,2)=P1(2)+delt(i,2);
P2=Triangle2(A,B,C,tag_x(2),tag_y(2));
PL2(i,1)=P2(1)+delt(i,1);
PL2(i,2)=P2(2)+delt(i,2);
P3=Triangle2(A,B,C,tag_x(3),tag_y(3));
PL3(i,1)=P3(1)+delt(i,1);
PL3(i,2)=P3(2)+delt(i,2);
P4=Triangle2(A,B,C,tag_x(4),tag_y(4));
PL4(i,1)=P4(1)+delt(i,1);
PL4(i,2)=P4(2)+delt(i,2);
P5=Triangle2(A,B,C,tag_x(5),tag_y(5));
PL5(i,1)=P5(1)+delt(i,1);
PL5(i,2)=P5(2)+delt(i,2);
P6=Triangle2(A,B,C,tag_x(6),tag_y(6));
PL6(i,1)=P6(1)+delt(i,1);
PL6(i,2)=P6(2)+delt(i,2);
P7=Triangle2(A,B,C,tag_x(7),tag_y(7));
PL7(i,1)=P7(1)+delt(i,1);
PL7(i,2)=P7(2)+delt(i,2);
P8=Triangle2(A,B,C,tag_x(8),tag_y(8));
PL8(i,1)=P8(1)+delt(i,1);
PL8(i,2)=P8(2)+delt(i,2);
P9=Triangle2(A,B,C,tag_x(9),tag_y(9));
PL9(i,1)=P9(1)+delt(i,1);
PL9(i,2)=P9(2)+delt(i,2);
P10=Triangle2(A,B,C,tag_x(10),tag_y(10));
PL10(i,1)=P10(1)+delt(i,1);
PL10(i,2)=P10(2)+delt(i,2);
end %maxrix(i,j,:)表示第i个reader 与第j个tag的p阵 dist(i,j)的距离
m1(r)=(PL1(1,1)/dist1(1,1)+PL1(2,1)/dist1(2,1)+PL1(3,1)/dist1(3,1)+PL1(4,1)/dist1(4,1))/(1/dist1(1,1)+1/dist1(2,1)+1/dist1(3,1)+1/dist1(4,1));
z1(r)=(PL1(1,2)/dist1(1,1)+PL1(2,2)/dist1(2,1)+PL1(3,2)/dist1(3,1)+PL1(4,2)/dist1(4,1))/(1/dist1(1,1)+1/dist1(2,1)+1/dist1(3,1)+1/dist1(4,1));
m2(r)=(PL2(1,1)/dist1(1,2)+PL2(2,1)/dist1(2,2)+PL2(3,1)/dist1(3,2)+PL2(4,1)/dist1(4,2))/(1/dist1(1,2)+1/dist1(2,2)+1/dist1(3,2)+1/dist1(4,2));
z2(r)=(PL2(1,2)/dist1(1,2)+PL2(2,2)/dist1(2,2)+PL2(3,2)/dist1(3,2)+PL2(4,2)/dist1(4,2))/(1/dist1(1,2)+1/dist1(2,2)+1/dist1(3,2)+1/dist1(4,2));
m3(r)=(PL3(1,1)/dist1(1,3)+PL3(2,1)/dist1(2,3)+PL3(3,1)/dist1(3,3)+PL3(4,1)/dist1(4,3))/(1/dist1(1,3)+1/dist1(2,3)+1/dist1(3,3)+1/dist1(4,3));
z3(r)=(PL3(1,2)/dist1(1,3)+PL3(2,2)/dist1(2,3)+PL3(3,2)/dist1(3,3)+PL3(4,2)/dist1(4,3))/(1/dist1(1,3)+1/dist1(2,3)+1/dist1(3,3)+1/dist1(4,3));
m4(r)=(PL4(1,1)/dist1(1,4)+PL4(2,1)/dist1(2,4)+PL4(3,1)/dist1(3,4)+PL4(4,1)/dist1(4,4))/(1/dist1(1,4)+1/dist1(2,4)+1/dist1(3,4)+1/dist1(4,4));
z4(r)=(PL4(1,2)/dist1(1,4)+PL4(2,2)/dist1(2,4)+PL4(3,2)/dist1(3,4)+PL4(4,2)/dist1(4,4))/(1/dist1(1,4)+1/dist1(2,4)+1/dist1(3,4)+1/dist1(4,4));
m5(r)=(PL5(1,1)/dist1(1,5)+PL5(2,1)/dist1(2,5)+PL5(3,1)/dist1(3,5)+PL5(4,1)/dist1(4,5))/(1/dist1(1,5)+1/dist1(2,5)+1/dist1(3,5)+1/dist1(4,5));
z5(r)=(PL5(1,2)/dist1(1,5)+PL5(2,2)/dist1(2,5)+PL5(3,2)/dist1(3,5)+PL5(4,2)/dist1(4,5))/(1/dist1(1,5)+1/dist1(2,5)+1/dist1(3,5)+1/dist1(4,5));
m6(r)=(PL6(1,1)/dist1(1,6)+PL6(2,1)/dist1(2,6)+PL6(3,1)/dist1(3,6)+PL6(4,1)/dist1(4,6))/(1/dist1(1,6)+1/dist1(2,6)+1/dist1(3,6)+1/dist1(4,6));
z6(r)=(PL6(1,2)/dist1(1,6)+PL6(2,2)/dist1(2,6)+PL6(3,2)/dist1(3,6)+PL6(4,2)/dist1(4,6))/(1/dist1(1,6)+1/dist1(2,6)+1/dist1(3,6)+1/dist1(4,6));
m7(r)=(PL7(1,1)/dist1(1,7)+PL7(2,1)/dist1(2,7)+PL7(3,1)/dist1(3,7)+PL7(4,1)/dist1(4,7))/(1/dist1(1,7)+1/dist1(2,7)+1/dist1(3,7)+1/dist1(4,7));
z7(r)=(PL7(1,2)/dist1(1,7)+PL7(2,2)/dist1(2,7)+PL7(3,2)/dist1(3,7)+PL7(4,2)/dist1(4,7))/(1/dist1(1,7)+1/dist1(2,7)+1/dist1(3,7)+1/dist1(4,7));
m8(r)=(PL8(1,1)/dist1(1,8)+PL8(2,1)/dist1(2,8)+PL8(3,1)/dist1(3,8)+PL8(4,1)/dist1(4,8))/(1/dist1(1,8)+1/dist1(2,8)+1/dist1(3,8)+1/dist1(4,8));
z8(r)=(PL8(1,2)/dist1(1,8)+PL8(2,2)/dist1(2,8)+PL8(3,2)/dist1(3,8)+PL8(4,2)/dist1(4,8))/(1/dist1(1,8)+1/dist1(2,8)+1/dist1(3,8)+1/dist1(4,8));
m9(r)=(PL9(1,1)/dist1(1,9)+PL9(2,1)/dist1(2,9)+PL9(3,1)/dist1(3,9)+PL9(4,1)/dist1(4,9))/(1/dist1(1,9)+1/dist1(2,9)+1/dist1(3,9)+1/dist1(4,9));
z9(r)=(PL9(1,2)/dist1(1,9)+PL9(2,2)/dist1(2,9)+PL9(3,2)/dist1(3,9)+PL9(4,2)/dist1(4,9))/(1/dist1(1,9)+1/dist1(2,9)+1/dist1(3,9)+1/dist1(4,9));
m10(r)=(PL10(1,1)/dist1(1,10)+PL10(2,1)/dist1(2,10)+PL10(3,1)/dist1(3,10)+PL10(4,1)/dist1(4,10))/(1/dist1(1,10)+1/dist1(2,10)+1/dist1(3,10)+1/dist1(4,10));
z10(r)=(PL10(1,2)/dist1(1,10)+PL10(2,2)/dist1(2,10)+PL10(3,2)/dist1(3,10)+PL10(4,2)/dist1(4,10))/(1/dist1(1,10)+1/dist1(2,10)+1/dist1(3,10)+1/dist1(4,10));
r=r+1;
end
get_x(1)=mean(m1);
get_y(1)=mean(z1);
get_x(2)=mean(m2);
get_y(2)=mean(z2);
get_x(3)=mean(m3);
get_y(3)=mean(z3);
get_x(4)=mean(m4);
get_y(4)=mean(z4);
get_x(5)=mean(m5);
get_y(5)=mean(z5);
get_x(6)=mean(m6);
get_y(6)=mean(z6);
get_x(7)=mean(m7);
get_y(7)=mean(z7);
get_x(8)=mean(m8);
get_y(8)=mean(z8);
get_x(9)=mean(m9);
get_y(9)=mean(z9);
get_x(10)=mean(m10);
get_y(10)=mean(z10);
plot(reader_x,reader_y,'kp',get_x,get_y,'k*',tag_x,tag_y,'bo'),title('☆为参考点,*为测试结果,o为目标点'),ylabel('y /m'),xlabel('x /m');grid on
set(gca,'Xtick',[-1:1:11],'Ytick',[-1:1:11]);
hold on
plot([tag_x(1);get_x(1)],[tag_y(1);get_y(1)],'LineWidth',2);
plot([tag_x(2);get_x(2)],[tag_y(2);get_y(2)],'LineWidth',2);
plot([tag_x(3);get_x(3)],[tag_y(3);get_y(3)],'LineWidth',2);
plot([tag_x(4);get_x(4)],[tag_y(4);get_y(4)],'LineWidth',2);
plot([tag_x(5);get_x(5)],[tag_y(5);get_y(5)