?%?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個文件信息