室内三维定位算法，伸缩因子

%%%%%%%%在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