%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 基于RSSI的目标定位算法
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function rssiEstimate
Length=100;
Width=100;
Node_number=3;
for i=1:Node_number
Node(i).x=Width*rand;
Node(i).y=Length*rand;
Node(i).D=Node(i).x^2+Node(i).y^2;
end
Target.x=Width*rand;
Target.y=Length*rand;
X=[];
Z=[];
for i=1:Node_number
for t=1:10
[d]=DIST(Node(i),Target);
Z(i,t)=GetRssiValue(d)
end
end
ZZ=[];
for i=1:Node_number
ZZ(i)=sum(Z(i,:))/10;
end
Zd=[];
for i=1:Node_number
Zd(i)=GetDistByRssi(ZZ(i));
end
H=[];b=[];
for i=2:Node_number
H=[H;2*(Node(i).x-Node(1).x),2*(Node(i).y-Node(1).y)];
b=[b;Zd(1)^2-Zd(i)^2+Node(i).D-Node(1).D];
end
Estimate=inv(H'*H)*H'*b;
Est_Target.x=Estimate(1);Est_Target.y=Estimate(2);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure
hold on;box on;axis([0 120 0 120]);
for i=1:Node_number
h1=plot(Node(i).x,Node(i).y,'ko','MarkerFace','g','MarkerSize',10);
text(Node(i).x+2,Node(i).y,['Node ',num2str(i)]);
end
h2=plot(Target.x,Target.y,'k^','MarkerFace','b','MarkerSize',10);
h3=plot(Est_Target.x,Est_Target.y,'ks','MarkerFace','r','MarkerSize',10);
line([Target.x,Est_Target.x],[Target.y,Est_Target.y],'Color','k');
legend([h1,h2,h3],'Observation Station','Target Postion','Estimate Postion');
[Error_Dist]=DIST(Est_Target,Target);
xlabel(['error=',num2str(Error_Dist),'m']);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [dist]=DIST(A,B)
dist=sqrt((A.x-B.x)^2+(A.y-B.y)^2);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function value=GetRssiValue(d)
A=-42;
n=2;
value=A-10*n*log10(d);
Q=5;
value=value+sqrt(Q)*randn;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function value=GetDistByRssi(rssi)
A=-42;
n=2;
value=10^((A-rssi)/10/n);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
