一、仿真要求
要求一：編寫兩個函數TDOA_CHAN和TDOA_Taylor得到位置的估計。
要求二：用RMSE實現兩種算法的性能比較, 得到兩種算法的RMSE曲線對比圖，橫座標爲噪聲方差，縱座標爲RMSE。
二、仿真方案的設計、仿真結果及結論
1.仿真方案的設計
TDOA基本定位過程如下
2.仿真結果
3.仿真結論
三、主要仿真代碼
1.main函數中的代碼如下：
%TDOA定位算法的仿真
clear all;
clc;
%定義四個參與基站的座標位置
BS1=[0,0];BS2=[500,0];BS3=[500,500];BS4=[0,500];
%BS5=[600,500];
%移動臺MS的初始估計位置
MS=[50,100];
std_var=[1e-2,5e-2,1e-1,5e-1,1];%範圍
A=[BS1;BS2;BS3;BS4];%矩陣A包含4個初始座標
number=10000;
for j=1:length(std_var)%循環
error1=0;error2=0;%初始誤差置爲0
std_var1=std_var(j);%令std_var1等於當前數組的值
for i=1:number%多次循環
r1=A-ones(4,1)*MS;
r2=(sum(r1.^2,2)).^(1/2);%計算移動臺到各個基站的實際距離
r=r2(2:end,:)-ones(3,1)*r2(1,:)+std_var1*randn(3,1); %表示從[2,i]開始MS與基站i和基站1的距離差
sigma=std_var1^2;
theta1=TDOA_CHAN(A,r,sigma);%調用TDOA_CHAN函數
theta2=TDOA_Taylor(A,r,sigma);%調用TDOA_Talor函數
error1=error1+norm(MS-theta1)^2;%移動臺MS估計位置與計算的到的距離的平方
error2=error2+norm(MS-theta2)^2;
end
RMSE1(j)=(error1/number)^(1/2); %均方根誤差
RMSE2(j)=(error2/number)^(1/2);
end
% 繪圖
semilogx(std_var,RMSE1,'-O',std_var,RMSE2,'-s');% x軸取對數，X軸範圍是1e-2到1,Y軸的範圍是變動的
xlabel('測量噪聲的標準差(m)');
ylabel('RMSE');
legend('TDOA-CHAN','TDOA-Taylor');
2.TDOA_CHAN函數中代碼如下：
function theta = TDOA_CHAN(A,p,sigma)
% A是BSs的座標
% p是距離測量
% sigma是TOA測量的方差
[m,~]=size(A);%size得到A的行列數賦值給[m,~]，~表示佔位，就是隻要行m的值
k=sum(A.^2,2);%矩陣A每個元素分別平方，得到新的矩陣，再按行求和，爲矩陣K
G1=[A(2:end,:)+ones(m-1,1)*A(1,:),p]; %得到Xm1,Ym1,Rm1,的值，m取值[2,i],構建矩陣Ga
h1=1/2*(p.^2-k(2:end,:)+ones(m-1,1)*k(1,:)); %構建矩陣h
Q=diag(ones(m-1,1)*sigma);%構建TDOA的協方差矩陣
% 初始預計值
theta0=inv(G1'*inv(Q)*G1)*G1'*inv(Q)*h1;%通過一次wls算法進行求解
s=A(2:end,:)-ones(m-1,1)*theta0(1:2,:)';
d=sum(s.^2,2);%矩陣s每個元素分別平方，得到新矩陣，再行求和，最爲矩陣d
B1=diag(d.^(1/2));
cov1=B1*Q*B1;
% 第一次wls
theta1=inv(G1'*inv(cov1)*G1)*G1'*inv(cov1)*h1;%進行第一次wls計算
cov_theta1=inv(G1'*inv(cov1)*G1);%得到theta1的協方差矩陣
%第二次wls
G2=[1,0;0,1;1,1];%構建Ga'
h2=[(theta1(1,1)-A(1,1))^2;(theta1(2,1)-A(1,2))^2;theta1(3,1)^2];%構建h'
B2=diag([theta1(1,1)-A(1,1),theta1(2,1)-A(1,2),theta1(3,1)]);%構建b'
cov2=4*B2*cov_theta1*B2;%得到誤差矢量的協方差矩陣
theta2=inv(G2'*inv(cov2)*G2)*G2'*inv(cov2)*h2;%運用最大似然估計得到
theta=theta2.^(1/2)+[A(1,1);A(1,2)];%得到MS位置的估計值座標，以及符號
theta=theta';%轉換爲（x,y）形式,得到(x0,y0)
3.TDOA_Taylor函數代碼如下：
function theta = TDOA_Taylor(A,p,sigma)
% A是BSs的座標
% p是距離測量
% sigma是TOA測量的方差
% 初始預計值
theta0=TDOA_CHAN(A,p,sigma);%調用TDOA_CHAN得到一個初始的估計位置
delta=norm(theta0);%得到矩陣範數
while norm(delta)>1e-2%得到足夠小的值(0.01)
[m,~]=size(A);%size得到A的行列數賦值給[m,~]，~表示佔位，就是隻要行m的值
d=sum((A-ones(m,1)*theta0).^2,2);
R=d.^(1/2);
G1=ones(m-1,1)*(A(1,1)-theta0(1,1))/R(1,1)-(A(2:m,1)-theta0(1,1))./R(2:m,:);
G2=ones(m-1,1)*(A(1,2)-theta0(1,2))/R(1,1)-(A(2:m,2)-theta0(1,2))./R(2:m,:);
G=[G1,G2];%構建Gt
h=p-(R(2:m,:)-ones(m-1,1)*R(1,:));%構建ht
Q=diag(ones(m-1,1)*sigma);%TDOA測量值的協方差矩陣
delta=inv(G'*inv(Q)*G)*G'*inv(Q)*h;% wls加權最小二乘解
theta0=theta0+delta';%累加,不滿足條件繼續循環
end
theta=theta0;%輸出對應的位置座標
以上在matlab環境下進行的TDOA定位算法性能仿真，與我們老師講的基本是一致的，有疑問歡迎評論，加油！