dingwei.zip_dingwei_三边法 定位_无线传感器_无线传感器网络 MATLAB_无线传感器网络 定位 Matla

close all %关闭所有非0的元素 clc %关闭命令窗口 clear %释放存储空间 disp('请输入已知节点数n(n>3);'); n=input('input n='); if n<3; %判断节点数是否满足要求 disp('输入节点数错'); return; %用break来终止时，注意break只用于for或者while的循环终止 end for m=1:100; %----------随机产生n个点----------- for i=1:n x(i)=10*rand(1); % rand（1）是在[0,1]间产生1行1列的随机矩阵，再*10，即把范围扩大到[0,10]产生1个随机数 y(i)=10*rand(1); % 产生一个随机数（大小为0~10） end grid; %显示网格线 %---------随机产生一个点P[x(n+1),y(n+1)]------ x(n+1)=10*rand(1); y(n+1)=10*rand(1); P=[x(n+1),y(n+1)]; %--------计算P到各已知点的距离d1~dn--------------- for i=1:n; d(i)=sqrt((x(n+1)-x(i))^2+(y(n+1)-y(i))^2); end d(1,i)=d(i); %点P到各已知点的距离矩阵 %--------给距离di，加随机误差，变成Di--------- for i=1:n; D(i)=d(i)+2*rand(1); %加[0,0.1]的随机误差 end D(1,i)=D(i); %------------根据极大似然估计定位出P点的坐标[xp,yp]---------------- for i=1:n-1 M(i,1)=[2*(x(i)-x(n))]; M(i,2)=[2*(y(i)-y(n))]; end for i=1:n-1; N(i,1)=[x(i)^2-x(n)^2+y(i)^2-y(n)^2+D(n)^2-D(i)^2]; end T=M'; X=inv(T*M)*T*N; xp=X([1]); %提取X矩阵的第一行，即算出的点P的横坐标 yp=X([2]); %提取X矩阵的第二行，即算出的点P的纵坐标 %------------计算误差，定义误差为算出的P点与真实的P点之间的距离-------- error=sqrt((xp-x(n+1))^2+(yp-y(n+1))^2); %------------重复100次------------------ errorList(m)=error; end graNum=1:100; plot(graNum,errorList); hold on %-----------------三边法---------------- for m = 1:100 %----------随机产生三个点----------- for i=1:3 x(i)=10*rand(1); % rand（1）是在[0,1]间产生1行1列的随机矩阵，再*10，即把范围扩大到[0,10]产生1个随机数 y(i)=10*rand(1); % 产生一个随机数（大小为0~10） end grid; %显示网格线 A=[x(1),y(1)]; B=[x(2),y(2)]; C=[x(3),y(3)]; %---------在三角形ABC内随机产生一个点D------ k = 1; while (k==1) x(4)=10*rand(1); y(4)=10*rand(1); a4=(x(4)-x(3))/(x(2)-x(3))-(y(4)-y(3))/(y(2)-y(3)); a1=(x(1)-x(3))/(x(2)-x(3))-(y(1)-y(3))/(y(2)-y(3)); b4=(x(4)-x(1))/(x(3)-x(1))-(y(4)-y(1))/(y(3)-y(1)); b2=(x(2)-x(1))/(x(3)-x(1))-(y(2)-y(1))/(y(3)-y(1)); c4=(x(4)-x(1))/(x(2)-x(1))-(y(4)-y(1))/(y(2)-y(1)); c3=(x(3)-x(1))/(x(2)-x(1))-(y(3)-y(1))/(y(2)-y(1)); if (a4*a1>0)&&(b4*b2>0)&&(c4*c3>0); k=0; P=[x(4),y(4)]; %三角形内的点P的坐标 end end %--------计算AP,BP,CP的距离d1,d2,d3--------------- for i=1:3; d(i)=sqrt((x(4)-x(i))^2+(y(4)-y(i))^2); end d=[d(1),d(2),d(3)]; %点P到点A,B,C的距离矩阵 %--------给距离d1,d2,d3，加随机误差，变成D1,D2,D3--------- for i=1:3; D(i)=d(i)+2*rand(1); %加[0,0.1]的随机误差 end D=[D(1),D(2),D(3)]; %------------根据三边法定位出P点的坐标---------------- M=[2*(x(1)-x(3)),2*(y(1)-y(3));2*(x(2)-x(3)),2*(y(2)-y(3))]; N=[x(1)^2-x(3)^2+y(1)^2-y(3)^2+D(3)^2-D(1)^2;x(2)^2-x(3)^2+y(2)^2-y(3)^2+D(3)^2-D(2)^2]; T=M'; X=inv(T*M)*T*N; x4=X([1]); %提取X矩阵的第一行，即算出的点P的横坐标 y4=X([2]); %提取X矩阵的第二行，即算出的点P的纵坐标 %------------计算误差，定义误差为算出的P点与真实的P点之间的距离-------- error=sqrt((x4-x(4))^2+(y4-y(4))^2); %-------------重复100次----------------- errorList(m) = error; end graNum = 1:100; plot(graNum, errorList,'r')