function chantaylor()
chantaylorCDFjs = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0];
M = 4;
for i=1:5000
Chantms = [5,10];
Chantx=Chantms(1);
Chanty=Chantms(2);
Chantc=3*10^8;
ChantX=[0,0,-45,-45,0,45,45];
ChantY=[0,51.96,25.98,-25.98,-59.16,-25.98,25.98];
Chantbasestx=ChantX(1:M);
Chantbasesty=ChantY(1:M);
ChantN=length(Chantbasestx);
%ChantStandarddeviation=[0.1,0.2,0.3,0.4,0.5,0.6,0.7,0.8,0.9,1,1.1,1.2,1.3,1.4,1.5,1.6,1.7,1.8,1.9,2];
ChantStandarddeviation = 1;
Chantri1=[];
Chantxi1=[];
Chantyi1=[];
Chantk=[];
Chanth=[];
ChantGa=[];
chantaylorBSN = M;
chantaylorBS=[0,0,-45,-45,0,45,45;0,51.96,25.98,-25.98,-59.16,-25.98,25.98];
chantaylornoise = 1;
chantaylorQ = eye(chantaylorBSN-1);
chanjqxsr = 0;
chanjqxs = 0;
chantaylorjqxsr = 0;
chantaylorjqhxfz = 0;
chantaylordwwc = [0,0.2,0.4,0.6,0.8,1,1.2,1.4,1.6,1.8,2,2.2,2.4,2.6,2.8,3];
for i=2:ChantN
Chantxi1(i-1)=Chantbasestx(i)-Chantbasestx(1);
Chantyi1(i-1)=Chantbasesty(i)-Chantbasesty(1);
end
for i=1:ChantN
Chantk(i)=(Chantbasestx(i))^2+(Chantbasesty(i))^2;
end
for j0=1:length(chantaylordwwc)
for i=2:ChantN
Chantri1(i-1)=sqrt((Chantbasestx(i)-Chantx)^2+(Chantbasesty(i)-Chanty)^2)- sqrt((Chantbasestx(1)-Chantx)^2+(Chantbasesty(1)-Chanty)^2)-ChantStandarddeviation*randn(1);
end
for i=2:ChantN
Chanth(i-1)=0.5*((Chantri1(i-1))^2-Chantk(i)+Chantk(1));
end
for i=1:3
for j=2:ChantN
switch i,
case 1,
ChantGa(j-1,i)=-Chantxi1(j-1);
case 2,
ChantGa(j-1,i)=-Chantyi1(j-1);
case 3,
ChantGa(j-1,i)=-Chantri1(j-1);
end
end
end
ChantQ=zeros(ChantN-1,ChantN-1);
for i=1:ChantN-1
ChantQ(i,i)=(ChantStandarddeviation)^2;
end
ChantZa=inv(ChantGa'*inv(ChantQ)*ChantGa)*ChantGa'*inv(ChantQ)*Chanth';
ChantB1=[];
for i=1:ChantN-1
ChantB1(i,i)=sqrt((Chantbasestx(i+1)-ChantZa(1))^2+(Chantbasesty(i+1)-ChantZa(2))^2);
end
ChantP1=Chantc^2*ChantB1*ChantQ*ChantB1;
ChantZa1=inv(ChantGa'*inv(ChantP1)*ChantGa)*ChantGa'*inv(ChantP1)*Chanth';
ChantC=inv(ChantGa'*inv(ChantQ)*ChantGa);
Chanth1=[(ChantZa1(1)-Chantbasestx(1))^2;(ChantZa1(2)-Chantbasesty(1))^2;(ChantZa1(3))^2];
ChantGa1=[1,0;0,1;1,1];
Chantr1=sqrt((Chantbasestx(1)-ChantZa1(1))^2+(Chantbasesty(1)-ChantZa1(2))^2);
ChantB2=[ChantZa1(1)-Chantbasestx(1),0,0;0,ChantZa1(2)-Chantbasesty(1),0;0,0,Chantr1];
ChantP2=4*ChantB2*ChantC*ChantB2;
ChantZa2=inv(ChantGa1'*inv(ChantP2)*ChantGa1)*ChantGa1'*inv(ChantP2)*Chanth1;
Chantms0=sqrt(ChantZa2)+[Chantbasestx(1);Chantbasesty(1)];
chantaylorMS = [Chantms0(1),Chantms0(2)];
chantayloriEP = chantaylorMS;
chantaylorx = 0;
chantaylory = 0;
chantayloraaa = 10;
chantaylorbbb = 10;
for i = 1: chantaylorBSN,
chantaylorMeaDist(i) = sqrt((chantaylorMS(1) - chantaylorBS(1,i))^2 + (chantaylorMS(2) - chantaylorBS(2,i))^2);
end
for i = 1: chantaylorBSN-1,
chantaylorh0(i) = chantaylorMeaDist(i+1) - chantaylorMeaDist(1) + chantaylornoise*randn(1);
end
while( chantayloraaa + chantaylorbbb >10)
chantaylorR1 = sqrt(chantayloriEP(1)^2 + chantayloriEP(2)^2);
for i = 1: chantaylorBSN-1,
chantaylorR(i) = sqrt((chantayloriEP(1) - chantaylorBS(1,i+1))^2 + (chantayloriEP(2) - chantaylorBS(2,i+1))^2);
end
for i = 1: chantaylorBSN-1,
chantaylorh(i) = chantaylorh0(i) - (chantaylorR(i) - chantaylorR1);
end
chantaylorht = chantaylorh';
for i = 1: chantaylorBSN-1,
chantaylorGt(i, 1) = -chantayloriEP(1)/chantaylorR1 - (chantaylorBS(1, i+1) - chantayloriEP(2))/chantaylorR(i);
chantaylorGt(i, 2) = -chantayloriEP(2)/chantaylorR1 - (chantaylorBS(2, i+1) - chantayloriEP(2))/chantaylorR(i);
end
chantaylordelt = inv(chantaylorGt'*inv(chantaylorQ)*chantaylorGt)*chantaylorGt'*inv(chantaylorQ)*chantaylorht;
chantayloraaa = abs(chantaylordelt(1));
chantaylorbbb = abs(chantaylordelt(2));
chantaylorEP = chantayloriEP + chantaylordelt';
chantayloriEP = chantaylorEP;
end
if ((Chantms0(1)-5)^2)+((Chantms0(2)-10)^2) > ((chantayloriEP(1)-5)^2)+((chantayloriEP(2)-10)^2)
chantaylorx = chantayloriEP(1);
chantaylory = chantayloriEP(2);
else
chantaylorx = Chantms0(1);
chantaylory = Chantms0(2);
end
chantaylorrmse = sqrt(((chantaylorx-5)^2)+((chantaylory-10)^2));
for i=2:M
chanderk = sqrt(((Chantms0(1)-ChantX(i))^2)+((Chantms0(2)-ChantY(i))^2))-sqrt(((Chantms0(1))^2)+((Chantms0(2))^2));
chandezr = sqrt(((5-ChantX(i))^2)+((10-ChantY(i))^2))-sqrt(125);
chandeltr = (chandezr-chanderk)^2;
chanjqxsr = chanjqxsr + chandeltr;
end
chanjqxs = chanjqxsr / M;
for i=2:M
chantaylorderk = sqrt((chantaylorx-ChantX(i))^2+(chantaylory-ChantY(i))^2)-sqrt((chantaylorx)^2+(chantaylory)^2);
chantaylordezr = sqrt((5-ChantX(i))^2+(10-ChantY(i))^2)-sqrt(125);
chantaylordeltr = (chantaylordezr-chantaylorderk)^2;
chantaylorjqxsr = chantaylorjqxsr + chantaylordeltr;
end
chantaylorjqxs = chantaylorjqxsr / M;
chantaylorjqhx = ((Chantms0(1)/chanjqxs) + (chantaylorx/chantaylorjqxs))/((1/chanjqxs) + (1/chantaylorjqxs));
chantaylorjqhy = ((Chantms0(2)/chanjqxs) + (chantaylory/chantaylorjqxs))/((1/chanjqxs)+(1/chantaylorjqxs));
chantaylorjqrmse = sqrt(((5-chantaylorjqhx)^2)+((10-chantaylorjqhy)^2));
if chantaylorjqrmse < chantaylordwwc(j0)
chantaylorCDFjs(j0) = chantaylorCDFjs(j0) + 1;
end
end
end
figure
plot(chantaylordwwc,chantaylorCDFjs/5000,'^--r')
axis([0,3,0,1]);
legend('chan-tanlor混合加权算法');
grid on;
hold on;
ylabel('CDF');
xlabel('定位误差/m');
title('TDOA下4个基站参与定位')
