%主函数
clear all;
clc;
BS1=[0,0];
BS2=[500,0];
BS3=[500,500];
BS4=[0,500];
MS=[100,100]; %移动台MS的估计位置
std_var=[1e-2,1e-1,1,1e1,1e2]; %范围矩阵
A=[BS1;BS2;BS3;BS4];
number=10000;
for j=1:length(std_var) %从1循环到std_var的长度
error1=0;%初始误差为0
error2=0; %初始误差为0
std_var1=std_var(j);
for i=1:number %多次循环
r1=A-ones(4,1)*MS; %矩阵A减去4*1的全一矩阵乘以MS
%r1=A-ones(3,1)*MS;
%r1=A-ones(5,1)*MS;
r2=sum(r1.^2,2); %矩阵r1每个元素分别平方，得到新矩阵，在行求和，最为矩阵r2
r=r2.^(1/2)+std_var1*randn(4,1); %从移动到位置MS发射信号到达基站i的TOA测量值
%r=r2.^(1/2)+std_var1*randn(3,1);
%r=r2.^(1/2)+std_var1*randn(5,1);
theta1=TOALLOP(A,r,1); % 调用TOALLOP函数
theta2=TOACHAN(A,r,std_var1^2); % 调用TOACHAN函数
error1=error1+norm(MS-theta1)^2; %norm是返回MS-theta1的最大奇异值，即max(svd(MS-theta1))，
error2=error2+norm(MS-theta2)^2; %移动台MS估计位置与计算的到的距离的平方
end
RMSE1(j)=(error1/number)^(1/2); %求TOALLOP均方根误差
RMSE2(j)=(error2/number)^(1/2);%求TOACHAN均方根误差
end
% plot
semilogx(std_var,RMSE1,'-O',std_var,RMSE2,'-s')
xlabel('测量噪声标准差（m） ');
legend('TOALLOP','TOACHAN');
ylabel('RMSE');
legend('TOA-LLOP','TOA-CHAN')
