本博客環境爲Matlab2018 ，軟件版本不同可能會有些出入，需要稍作修改。
TOA定位算法性能的仿真
要求一：編寫兩個函數TOA_LLOP和TOA_CHAN得到位置的估計。
要求二：用RMSE實現兩種算法的性能比較, 得到兩種算法的RMSE曲線對比圖，橫座標爲噪聲方差，縱座標爲RMSE。
主函數：
我的文件命名爲TOA.m
%TOA:
% %% the simulation of TOA localization algorithm
clear all;
clc;
%接收機的位置座標，簡單實驗就可採用少量樣本，精準實驗必須採用大量樣本數量
BS1=[0,0];
BS2=[500,0];
BS3=[500,500];
BS4=[0,500];
MS=[200,200]; %移動臺MS的估計位置
std_var=[1e-2,1e-1,1,1e1,1e2]; %範圍矩陣
A=[BS1;BS2;BS3;BS4]; %矩陣A包含基站的座標
%A=[BS1;BS2;BS3];
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;
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);
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') %x軸取對數，X軸範圍是1e-2到1e2,Y軸的範圍是變動的
xlabel('測量噪聲標準差（m） ');
legend('TOALLOP','TOACHAN');
ylabel('RMSE');
legend('TOA-LLOP','TOA-CHAN')
TOA-CHAN算法，我的文件命名爲TOACHAN.m
TOA_CHAN函數：
%TOACHAN:
function theta=TOACHAN(A,p,sigma)
%A是BBS的座標,P是範圍測量,sigma是TOA測量的方差
[m,~]=size(A); %size得到A的行列數賦值給[m,~]，~表示佔位，就是隻要行m的值！
k=sum(A.^2,2); %矩陣A每個元素分別平方，得到新矩陣，在行求和，最爲矩陣K
A1=[-2*A,ones(m,1)]; %-2乘以矩陣A與一個m*1的全一矩陣，組成新矩陣A1,也就是Gn
p1=p.^2-k; %得到h
B1=diag(2*p);%得到B,基站與移動臺的真實值
Q1=diag(ones(m,1)*sigma); %TOA協方差矩陣
cov1=B1*Q1*B1; %由於B1中有2，此處就可以省略4
theta1=inv(A1'*inv(cov1)*A1)*A1'*inv(cov1)*p1; %得到Za,因爲B中包含MS與個基站之間的距離，Q1是未知量，需進一步近似
cov_theta1=inv(A1'*inv(cov1)*A1); %上式採用擾動算法得
A2=[1,0;0,1;1,1]; %得到Ga‘
p2=[theta1(1,1)^2;
theta1(2,1)^2;
theta1(3,1)]; %得到h‘
B2=diag([2*theta1(1,1);
2*theta1(2,1);1]); %得到B’
cov2=B2*cov_theta1*B2; %誤差向量的協方差矩陣
theta2=inv(A2'*inv(cov2)*A2)*A2'*inv(cov2)*p2; %運用最大似然估計得到
theta=sign(theta1(1:2,:)).*theta2.^(1/2); %得到MS位置的估計值座標，以及符號
theta=theta';%轉換爲（x,y）形式
end
TOA-caffery算法，我的文件命名爲TOALLOP.m
TOA_LLOP函數：
%TOALLOP:
function theta=TOALLOP(A,p,j)
%A是BBS的座標,P是範圍測量,J是參考BS的索引
[m,~]=size(A); %size得到A的行列數賦值給[m,~]，~表示佔位，就是隻要行m的值！
k=sum(A.^2,2);%矩陣A每個元素分別平方，得到新矩陣，在行求和，最爲矩陣K
k1=k([1:j-1,j+1:m],:); %取出J行
A1=A([1:j-1,j+1:m],:); %取出J行
A2=A1-ones(m-1,1)*A(j,:); %得到D，就是j行與其餘行對應值相減
p1=p([1:j-1,j+1:m],:); %取出J行
p2=p(j).^2*ones(m-1,1)-p1.^2-(k(j)*ones(m-1,1)-k1); %得到b,(Rn*Rn-R1*R1-Kn+K1)其中Kn爲對應第n個x^2+y^2
theta=1/2*inv(A2'*A2)*A2'*p2; %利用最小二乘解，得
theta=theta';%轉換爲（x,y）形式
end
運行結果如下所示：
