jidadingwei.zip_DXD_极大似然估计定位算法_极大似然定位_非测距定位

close all %关闭所有非0的元素 clc %关闭命令窗口 clear %释放存储空间 disp('请输入已知节点数n(n>3);'); n=input('input n='); if n<3; %判断节点数是否满足要求 disp('输入节点数错'); return; %用break来终止时，注意break只用于for或者while的循环终止 end %----------随机产生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） scatter(x(i),y(i)); %画出圆圈，以a,b为圆心的横纵坐标。 hold on end grid; %显示网格线 %---------随机产生一个点P[x(n+1),y(n+1)]------ x(n+1)=10*rand(1); y(n+1)=10*rand(1); scatter(x(n+1),y(n+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的纵坐标 scatter(xp,yp); %------------计算误差，定义误差为算出的P点与真实的P点之间的距离-------- error=sqrt((xp-x(n+1))^2+(yp-y(n+1))^2)