用MATLAB编写，4个基站的基于TDOA的Chan-Taylor混合加权算法定位

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个基站参与定位')