最小二乘协作定位算法

clear all

close all

clc

n=5; %Connect with 1,3,4

T=10; %simulation times

Niter=15; %interation times

dim = 2; % Dimension

sigma=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:100

randn('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 accuracy

for 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 vector

de1=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;

else

e1 = (z2(1) - de1)*(-2)*(x1_temp - node1)/de1;

end

if(de2==0)

e2=0;

else

e2 = (z2(3) - de2)*(-2)*(x1_temp - node3)/de2;

end

if(de3==0)

e3=0;

else

e3 = (z2(4) - de3)*(-2)*(x1_temp - x2_temp)/de3;

end

%accumulate

e=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;

else

e4 = (z4(3) - de4)*(-2)*(x2_temp - node3)/de4;

end

if(de5==0)

e5=0;

else

e5 = (z4(5) - de5)*(-2)*(x2_temp - node5)/de5;

end

if(de6==0)

e6=0;

else

e6 = (z4(2) - de6)*(-2)*(x2_temp - x1_temp)/de6;

end

%accumulate

e0=e4+e5+e6;