%%%%%%%%在Chan算法中引入松弛變量：
%%%%%%%%通過加入松弛變量和采用搜索的算法，消除NLOS傳播對定位影響，不需要先驗信息，
%%%%%%%%但是由于其估計結果依賴于Chan算法估計的初值，當NLOS較大時，精度會顯著下降。

clear;
clc;
X0=1000;??????%%%%均勻隨機產生移動臺的位置
Y0=2000;
ErrVar=30;
NLOS=0.3;
c=300000000;
Zpp=[0;0];
RMSEp=0;
SampleNo=1;???????%%%%獨立運行的第幾次
SampleTotal=1000;???%%%%獨立運行的次數
NumRight=0;
RRx_1=0;
RRy_1=0;

X（1）=0;??????Y（1）=0;???%%%服務基站的坐標
X（2）=0;??????Y（2）=4000;???%%%各個基站的坐標??共采用四個基站
X（3）=3464;???Y（3）=2000;
X（4）=-3464;??Y（4）=2000;

m=1;
while?m<=4
????R（m）=sqrt（（X（m）-X0）^2+（Y（m）-Y0）^2）;
????m=m+1;
end;

while?SampleNo<=SampleTotal??????????????%%%%%%抽樣次數最大循環
m=1;
while?m<=4
????yy=exp（4*rand（1）/10）?;???????????%%??yy>0
????r（m）=R（m）+randn（1）*ErrVar+yy*NLOS*sqrt（R（m）/1000）*300;??????%%%%%%%增加??系統測量誤差?和??非視距傳播??影響后的?TOA測量值
????K（m）=X（m）^2+Y（m）^2;
????m=m+1;
end;
Ga=[-2*X（1）-2*Y（1）1;
????-2*X（2）-2*Y（2）1;
????-2*X（3）-2*Y（3）1;
????-2*X（4）-2*Y（4）1];
h=[r（1）^2-K（1）;
????r（2）^2-K（2）;
????r（3）^2-K（3）;
????r（4）^2-K（4）];
B=[r（1）000;
???0r（2）00;
???00r（3）0;
???000r（4）];
%%%Q=ErrVar*eye（6）;
Q=[r（1）-R（1）000;
????0r（2）-R（2）00;
????00r（3）-R（3）0;
????000r（4）-R（4）];

QQ=4*c^2*B*Q*B;
Za=inv（Ga‘*inv（QQ）*Ga）*Ga‘*inv（QQ）*h