基于TDOA定位的taylor级数展开法

用MATLAB编写，4个基站的基于TDOA的Taylor级数展开法定位。采取循环采样5000次，基站位置，标签节点位置，系统噪声标准差都已经预设置好，可以根据要求自己修改。本代码使用的衡量指标是累积分布函数CDF，也可以自己改成均方误差RMSE。下载后可以直接运行。可以用于TDOA定位算法的改进或者比较或者UWB定位都可以。

function taylor（）
taylora = 0;
taylorCDFjs = [0000000000000000];
for i=1:5000
M = 4;
Cms=[510];
Cx=Cms（1）;
Cy=Cms（2）;
Cc=3*10^8;
CX=[00-45-4504545];
CY=[051.9625.98-25.98-59.16-25.9825.98];
Cbasestx=CX（1:M）;
Cbasesty=CY（1:M）; 
CN=length（Cbasestx）; 
%CStandarddeviation=[0.10.20.30.40.50.60.70.80.911.11.21.31.41.51.61.71.81.92]; 
CStandarddeviation = 1;
Cri1=[];
Cxi1=[];
Cyi1=[]; 
Ck=[];
Ch=[];
CGa=[];
taylorBSN = M;
taylorBS=[00-45-4504545;051.9625.98-25.98-59.16-25.9825.98];
%taylornoise = [0.10.20.30.40.50.60.70.80.911.11.21.31.41.51.61.71.81.92];
taylornoise = 1;
taylorQ = eye（taylorBSN-1）;
taylordwwc = [00.20.40.60.811.21.41.61.822.22.42.62.83];
for i=2:CN
     Cxi1（i-1）=Cbasestx（i）-Cbasestx（1）;
    Cyi1（i-1）=Cbasesty（i）-Cbasesty（1）;
end  
for i=1:CN
    Ck（i）=（Cbasestx（i））^2+（Cbasesty（i））^2;
end  
Crmse=[];
for j0=1:length（taylordwwc）
   for i=2:CN
       %Cri1（i-1）=sqrt（（Cbasestx（i）-Cx）^2+（Cbasesty（i）-Cy）^2）- sqrt（（Cbasestx（1）-Cx）^2+（Cbasesty（1）-Cy）^2）-CStandarddeviation（j0）*rand（1）;
       Cri1（i-1）=sqrt（（Cbasestx（i）-Cx）^2+（Cbasesty（i）-Cy）^2）- sqrt（（Cbasestx（1）-Cx）^2+（Cbasesty（1）-Cy）^2）-CStandarddeviation*randn（1）;
   end
   for i=2:CN
        Ch（i-1）=0.5*（（Cri1（i-1））^2-Ck（i）+Ck（1））;
   end 
   for i=1:3
       for j=2:CN
           switch i
                  case 1
                        CGa（j-1i）=-Cxi1（j-1）;
                  case 2
                        CGa（j-1i）=-Cyi1（j-1）;
                 case 3
                        CGa（j-1i）=-Cri1（j-1）;
          end
      end
   end
   CQ=zeros（CN-1CN-1）;
   for i=1:CN-1
   %CQ（ii）=（CStandarddeviation（j0））^2; 
   CQ（ii）=（CStandarddeviation）^2; 
   end
   CZa=inv（CGa‘*inv（CQ）*CGa）*CGa‘*inv（CQ）*Ch‘;
   CB1=[];
   for i=1:CN-1
       CB1（ii）=sqrt（（Cbasestx（i+1）-CZa（1））^2+（Cbasesty（i+1）-CZa（2））^2）;
   end
       CP1=Cc^2*CB1*CQ*CB1;
       CZa1=inv（CGa‘*inv（CP1）*CGa）*CGa‘*inv（CP1）*Ch‘; 
       CC=inv（CGa‘*inv（CQ）*CGa）;
       Ch1=[（CZa1（1）-Cbasestx（1））^2;（CZa1（2）-Cbasesty（1））^2;（CZa1（3））^2];
       CGa1=[10;01;11];
       Cr1=sqrt（（Cbasestx（1）-CZa1（1））^2+（Cbasesty（1）-CZa1（2））^2）; 
       CB2=[CZa1（1）-Cbasestx（1）00;0CZa1（2）-Cbasesty（1）0;00Cr1];
       CP2=4*CB2*CC*CB2;
       CZa2=inv（CGa1‘*inv（CP2）*CGa1）*CGa1‘*inv（CP2）*Ch1;
       Cms0=sqrt（CZa2）+[Cbasestx（1）;Cbasesty（1）];
    %taylorMS = [CZa（1）CZa（2）];
%for j0=1:length（taylordwwc）
    taylorMS = [Cms0（1）Cms0（2）];
    tayloriEP = taylorMS;
    taylorx = 0;
    taylory = 0;
    tayloraaa = 10;
    taylorbbb = 10;

for i = 1: taylorBSN
    taylorMeaDist（i） = sqrt（（taylorMS（1） - taylorBS（1i））^2 + （taylorMS（2） - taylorBS（2i））^2）;
end
for i = 1: taylorBSN-1
    %taylorh0（i） = taylorMeaDist（i+1） - taylorMeaDist（1） + taylornoise（j0）*rand（1）;  
    taylorh0（i） = taylorMeaDist（i+1） - taylorMeaDist（1） + taylornoise*randn（1）;  
end


while（ taylora

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       4333  2020-09-27 08:47  taylor.m

----------- ---------  ---------- -----  ----

                 4333                    1