最小二乘协作定位算法

clear allclose allclcn=5; %Connect with 1,3,4T=10; %simulation timesNiter=15; %interation timesdim = 2; % Dimensionsigma=0.2;node1=[20,80]';node2=[20,70]';node3=[30,70]';node4=[20,60]';node5=[30,50]';%node2=[15,75]';%node4=[20,60]';%node = [20,90;rand(1,2);30,70;rand(1,2);30,50]; % 1,3,5 coordinate N=50 %node = [20,80;40,80;30,70;20,60]; % vague 4%z = zeros(dim,T,Niter,1);%z1 = zeros(dim,T,Niter,1);dieta_d = zeros(T,Niter); %contour z%d = zeros(dim,T,n);delat_2 = -0.25; %default delta==1%input node's coordinate%node = [20,80;1,1;30,70;1,1;30,50];z=zeros(dim,1);z1=zeros(dim,1);d12=norm((node1-node2),2);d32=norm((node3-node2),2);d42=norm((node4-node2),2);d34=norm((node3-node4),2);d54=norm((node5-node4),2);d24=norm((node2-node4),2);for test=1:100randn('state',sum(clock)*randn(1));z2(1)=d12+sigma*randn(1,1);randn('state',sum(clock)*randn(1));z2(3)=d32+sigma*randn(1,1);randn('state',sum(clock)*randn(1));z2(4)=d42+sigma*randn(1,1);randn('state',sum(clock)*randn(1));z4(3)=d34+sigma*randn(1,1);randn('state',sum(clock)*randn(1));z4(5)=d54+sigma*randn(1,1);randn('state',sum(clock)*randn(1));z4(2)=d24+sigma*randn(1,1);x1(:,1)=rand(2,1);x2(:,1)=rand(2,1);x1_temp=x1(:,1);x2_temp=x2(:,1);i=2;n=4;for t=2:T% xn(:,t,1,i)=xn(:,t-1,Niter-1,i);%xn(:,t,1,n)=xn(:,t-1,Niter-1,n);%xt(:,t-1,n);% besides Initialization, used the former product to improve the accuracyfor l=1:Niter%for j=1:5%i = 2;%if (j~=5)&&(i~=j)%e(t,l-1,j)=x(:,t,l-1,i)/norm((x(:,t,l-1,i)-x(:,t,l-1,j)),2)% Direction vectorde1=norm((x1_temp - node1),2);de2=norm((x1_temp - node3),2);de3=norm((x1_temp - x2_temp),2);%e(:,t,l-1,j) =(xn(:,t,l-1,i)-xn(:,t,l-1,j))/norm((xn(:,t,l-1,i)-xn(:,t,l-1,j)),2);if(de1==0)e1=0;elsee1 = (z2(1) - de1)*(-2)*(x1_temp - node1)/de1;endif(de2==0)e2=0;elsee2 = (z2(3) - de2)*(-2)*(x1_temp - node3)/de2;endif(de3==0)e3=0;elsee3 = (z2(4) - de3)*(-2)*(x1_temp - x2_temp)/de3;end%accumulatee=e1+e2+e3;%if (j~=1)&&(n~=j)%e(t,l-1,j)=x(:,t,l-1,i)/norm((x(:,t,l-1,i)-x(:,t,l-1,j)),2)% Direction vector% de=norm((x2_temp' - node(j,:)),2);%e(:,t,l-1,j) =(xn(:,t,l-1,i)-xn(:,t,l-1,j))/norm((xn(:,t,l-1,i)-xn(:,t,l-1,j)),2); % if(de==0)% e=0;%else% e = -2*(x2_temp(k) - node(j,k))/de;%end% end%accumulate%z1=z1+e'*(z4(j) - de);de4=norm((x2_temp - node3),2);de5=norm((x2_temp - node5),2);de6=norm((x2_temp - x1_temp),2);%e(:,t,l-1,j) =(xn(:,t,l-1,i)-xn(:,t,l-1,j))/norm((xn(:,t,l-1,i)-xn(:,t,l-1,j)),2); if(de4==0)e4=0;elsee4 = (z4(3) - de4)*(-2)*(x2_temp - node3)/de4;endif(de5==0)e5=0;elsee5 = (z4(5) - de5)*(-2)*(x2_temp - node5)/de5;endif(de6==0)e6=0;elsee6 = (z4(2) - de6)*(-2)*(x2_temp - x1_temp)/de6;end%accumulatee0=e4+e5+e6;x1(:,l) = x1_temp+delat_2*e;x2(:,l) = x2_temp+delat_2*e0;x1_temp=x1(:,l);x2_temp=x2(:,l);%if(norm((node2-xn(:,t,l,i)),2)==0)% dieta_d(t,l)=0;% else%dieta_d(t,l) = 1/((norm((node2-xn(:,t,l,i)),2))*(1.0e+14));%dieta_d(t,l)=1/(norm(node2(1)-xn(1,t,l,i))+norm(node2(2)-xn(2,t,l,i))* (1.0e+14));%if(dieta_d(t,l)>dieta_d(t,l-1))%output the max likelihood product%g=xn(:,t,l,i);% endendx1_time(:,t)=x1(:,Niter);x2_time(:,t)=x2(:,Niter);x1_temp=x1(:,Niter);x2_temp=x2(:,Niter);% xn(:,t,i) = xn(:,t,Niter-1,i);end%b=xn(:,t,l,i);x1_test(:,test)=x1_time(:,T);%x2_test(:,test)=x2_time(:,T);%h(:,test)=xn(:,T,Niter,i);end%fprintf(['y=[',num2str(dieta_d),']']);%fprintf('\n');x=x1_test;x1=node2;error_x1=x(1,:)-x1(1);error_y1=x(2,:)-x1(2);error_all=sqrt(error_x1.^2+error_y1.^2);max_error=max(error_all);min_error=min(error_all);a=0:max_error/100:max_error;for i=1:length(a)m=0;for j=1:length(error_all)if error_all(j)<=a(i)m=m+1;endendcount_error(i)=m;enderror_rate=count_error/length(error_all);plot(a,error_rate)%disp(g);%shaping%figure%[X,Y,Z]=griddata(x,y,dieta_d,linspace(0,60)',linspace(40,100),'v4'); %pcolor(X,Y,Z);shading interp%α²Êɫͼ%figure%contourf(X,Y,Z,37);%µÈ¸ßÏßͼ。