基于无线通信基站的室内三维定位问题-2016年研究生数学建模华为题目答案

华为杯”第十三届全国研究生数学建模竞赛-基于无线通信基站的室内三维定位问题：三维定位的MATLAB算法，包含源代码，测试结果，误差分析全套

 % BSN表示基站的个数，STATEA的前3列是基站的相对坐标，第四列为TDOA距离差
 %H=Ga*ZaZa为[xyz]
 %Q为TDOA协方差矩阵
 %V误差矢量协方差矩阵
function F=Chan_Taylor3D（BSNSTATEAbsbs1）
  
           for n=1:BSN
               K（n）=STATEA（n1）*STATEA（n1）+STATEA（n2）*STATEA（n2）+STATEA（n3）*STATEA（n3）;% x^2+y^2+z^2
           end
          
           for n=1:BSN-1     
               Xi1（n）=STATEA（n+11）-STATEA（11）;
               Yi1（n）=STATEA（n+12）-STATEA（12）;
               Zi1（n）=STATEA（n+13）-STATEA（13）;
           end    
           %取TDOA数据
           for n=2:BSN 
               ri1（n）=STATEA（n4）;
           end
           %构造H矩阵
           for n=1:BSN-1
               h（n1）=0.5*（ri1（n+1）.^2-K（n+1）+K（1））; 
           end    
           
           for n=1:BSN-1  %构造G矩阵
               Ga（n1）=-Xi1（n）;
               Ga（n2）=-Yi1（n）;
               Ga（n3）=-Zi1（n）;
               Ga（n4）=-ri1（n+1）;
           end 
          %Q为TDOA协方差矩阵
            %Q=eye（BSN-1）;    
          Q=（eye（BSN-1）+ones（BSN-1））/2;
          %第一次加权最小二值法（WLS）
            Za=pinv（Ga‘*pinv（Q）*Ga）*Ga‘*pinv（Q）*h;%pinv:求伪逆矩阵
            x1=Za（1）;
            y1=Za（2）;
            z1=Za（3）;
            r1=Za（4）;
         %% Za各元素独立，第二次WLS
            for n=1:BSN-1
                rno（n）=sqrt（（x1-STATEA（n+11））*（x1-STATEA（n+11））+（y1-STATEA（n+12））*（y1-STATEA（n+12））+（z1-STATEA（n+13））*（z1-STATEA（n+13）））;
            end
            
            B=diag（rno）;%，B矩阵
            % V为噪声的误差矢量协方差矩阵
            V=9*10^16*B*Q*B;
            % V代替Q，第二次WLS
            Za1=pinv（Ga‘*pinv（V）*Ga）*Ga‘*pinv（V）*h;          
            x10=Za1（1）;
            y10=Za1（2）;
            z10=Za1（3）; 
            r10=Za1（4）; 
            if r10<0     
                r10=0;
            end                   
         %% Za各元素不独立，第三次WLS
            covza=pinv（Ga‘*pinv（V）*Ga）;
            B2=diag（[x10 y10 z10 r10]）; 
            V2=4*B2*covza*B2;
            Ga1=[1 0 0;0 1 0;0 0 1;1 1 1];
            h1=[x10*x10;y10*y10;z10*z10;r10*r10]; 
           
            Zae=pinv（Ga1‘*pinv（V2）*Ga1）*Ga1‘*pinv（V2）*h1;

            Zp=sqrt（Zae）;
             if x10<0      %取正值为定位结果
             Zp1=-Zp（1）;
            else
             Zp1=Zp（1）;
            end
            if y10<0
              Zp2=-Zp（2）;
            else
              Zp2=Zp（2）;
            end
            if z10<0
              Zp3=-Zp（3）;
            else
              Zp3=Zp（3）;
            end
            F1=[Zp1;Zp2;Zp3]‘+bs（1:）;
            Zp1=F1（1）;
            Zp2=F1（2）;
            Zp3=F1（3）;
            %方程组的解出现虚数的处理
            m=isreal（Zp）==1;    %判断解是否含有虚数       
            if m<1              %如果含有虚数
               nn=find（imag（Zp））;%提取虚数解位置
               [aabb]=size（nn）;%判断虚数解个数
               if aa>1          
                   nnn=nn（11）;
                   nnn2=nn（21）;
                   nnn3=nn（endend）;
                   F1（nnn）=bs（1nnn）;
                   

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2017-10-23 23:03  第一题\
     目录           0  2017-10-23 23:03  第一题\第一题MATLAB程序\
     目录           0  2017-10-23 23:03  第一题\第一题MATLAB程序\测试用例程序\
     文件        5678  2016-09-21 18:28  第一题\第一题MATLAB程序\测试用例程序\Chan_Taylor3D.m
     文件        1616  2016-09-21 18:30  第一题\第一题MATLAB程序\测试用例程序\Chan_Taylor_main_3D.m
     目录           0  2017-10-23 23:03  第一题\第一题MATLAB程序\竞赛用例程序\
     文件        5711  2016-09-23 11:17  第一题\第一题MATLAB程序\竞赛用例程序\Chan_Taylor3D.m
     文件        1600  2016-09-21 19:40  第一题\第一题MATLAB程序\竞赛用例程序\Chan_Taylor_main_3D.m
     目录           0  2017-10-23 23:03  第一题\第一题输出结果\
     目录           0  2017-10-23 23:03  第一题\第一题输出结果\测试用例输出\
     目录           0  2017-10-23 23:03  第一题\第一题输出结果\测试用例输出\samplecase_01第一场景定位结果\
     文件       33190  2016-09-20 08:49  第一题\第一题输出结果\测试用例输出\samplecase_01第一场景定位结果\samplecase_01_均方根误差.png
     文件      123853  2016-09-21 19:38  第一题\第一题输出结果\测试用例输出\samplecase_01第一场景定位结果\samplecase_01三维定位图.fig
     文件       26827  2016-09-21 19:38  第一题\第一题输出结果\测试用例输出\samplecase_01第一场景定位结果\samplecase_01三维定位图.jpg
     目录           0  2017-10-23 23:03  第一题\第一题输出结果\测试用例输出\samplecase_02第二场景定位结果\
     文件      137018  2016-09-21 12:44  第一题\第一题输出结果\测试用例输出\samplecase_02第二场景定位结果\Z方向定位位置图.fig
     文件       61820  2016-09-21 12:45  第一题\第一题输出结果\测试用例输出\samplecase_02第二场景定位结果\Z方向定位位置图.jpg
     文件       34909  2016-09-20 08:20  第一题\第一题输出结果\测试用例输出\samplecase_02第二场景定位结果\samplecase_02_均方根误差.png
     文件      137211  2016-09-21 12:46  第一题\第一题输出结果\测试用例输出\samplecase_02第二场景定位结果\第二场景X-Y方向定位位置图.fig
     文件       83494  2016-09-21 12:46  第一题\第一题输出结果\测试用例输出\samplecase_02第二场景定位结果\第二场景X-Y方向定位位置图.jpg
     文件      137724  2016-09-21 12:43  第一题\第一题输出结果\测试用例输出\samplecase_02第二场景定位结果\第二场景三维定位图.fig
     文件       90825  2016-09-21 12:47  第一题\第一题输出结果\测试用例输出\samplecase_02第二场景定位结果\第二场景三维定位图.jpg
     目录           0  2017-10-23 23:03  第一题\第一题输出结果\测试用例输出\samplecase_03第一场景定位结果\
     文件      141766  2016-09-21 20:02  第一题\第一题输出结果\测试用例输出\samplecase_03第一场景定位结果\samplecase_03_三维定位图.fig
     文件       86843  2016-09-21 20:00  第一题\第一题输出结果\测试用例输出\samplecase_03第一场景定位结果\samplecase_03_三维定位图.jpg
     文件       28850  2016-09-20 09:04  第一题\第一题输出结果\测试用例输出\samplecase_03第一场景定位结果\samplecase_03_均方根误差.png
     目录           0  2017-10-23 23:03  第一题\第一题输出结果\测试用例输出\samplecase_04第一场景定位结果\
     文件       35228  2016-09-20 09:16  第一题\第一题输出结果\测试用例输出\samplecase_04第一场景定位结果\samplecase_04_均方根误差.png
     文件       61162  2016-09-20 09:18  第一题\第一题输出结果\测试用例输出\samplecase_04第一场景定位结果\samplecase_04_空间位置图.png
     目录           0  2017-10-23 23:03  第一题\第一题输出结果\测试用例输出\samplecase_05第一场景定位结果\
     文件       34922  2016-09-20 09:39  第一题\第一题输出结果\测试用例输出\samplecase_05第一场景定位结果\samplecase_05_均方根误差.png
............此处省略12个文件信息