利用MATLAB实现移动目标的TDOA/FDOA定位算法

移动目标中基于两步加权最小二乘方法的TDOA/FDOA算法

2019年09月27日更新：近期有许多同学在程序运行时总是出现各种各样的问题，主要集中在原始TDOA和FDOA数据的生成、Bias和RMSE结果的计算、CRLB下界的计算三个方面。本次把这三个部分的代码以及结果绘制的代码上传，希望能对大家有所帮助！

无源定位技术是接收站本身不发射信号，而是通过被动接收目标发射或者反射的信号完成定位的一种技术，它被广泛应用于传感器网络、无线通信、麦克风阵列和声纳等领域。
对于静止目标，常见的定位体制是测量目标信号到达各观测站的到达时间差(Time Difference of Arrival, TDOA)，并计算时差双曲线的交点以估计目标位置。当目标运动时，可同时利用TDOA和频差（Frequency Difference of Arrival, FDOA）观测量以估计目标位置和速度。
目前，对TDOA/FDOA定位定速的算法仍然在研究中，这里不进行太多的展开，仅介绍Dominic K.C. Ho教授于2004年提出了经典TDOA/FDOA算法——两步加权最小二乘算法(TSWLS：Two Stage/Step Constrained Weighted Least Squares Algorithm)，目前提出的很多新的算法均将该算法作为对比方法之一
Dominic KC Ho 教授简介 ；Dominic KC Ho/Google Scholar
本文参考文献：Ho K C, Xu W. An accurate algebraic solution for moving source location using TDOA and FDOA measurements[J]. IEEE Transactions on Signal Processing, 2004, 52(9): 2453-2463.

TDOA/FDOA量测模型

设所跟踪的移动目标的位置和速度分别为$u=[x,y,z{]}^T$和$\dot{u}=[\dot{x},\dot{y},\dot{z}{]}^T$，假设一共有$M$个传感器(或发射器，或基准站)的位置和速度分别为$s_i=[x_i,y_i,z_i{]}^T$和$\dot{s_i}=[\dot{x_i},\dot{y_i},\dot{z_i}{]}^T$，其中$i=1,2,...,M$。假设第1个传感器作为基准，那么所测量的第$i$个传感器与第1个传感器和移动目标之间的距离差值（Range Difference of Arrival, RDOA）可以表示为
$$r_{i,1}=d_i-d_1+n_{i,1}.......(1)$$其中，$d_i=\Vert u-s_i{\Vert }_2=\sqrt{(x-x_i{)}^2+(y-y_i{)}^2+(z-z_i{)}^2}$为第$i$个传感器与移动目标之间的真实距离，$n_{i,1}$为第$i$次RDOA测量的误差，假设该误差为零均值的高斯白噪声。
对公式（1）展开可以得到：$$2(s_i-s_1{)}^{T}u+2r_{i,1}{R}_1=({s_i}^T{s}_i-{s_1}^T{s}_1-{r_{i,1}}^2)+2d_i{n}_{i,1}.......(2)$$上式中，忽略掉误差的二阶项${n_{i,1}}^2$。为了利用到FDOA信息，对式（2）进行时间求导可以得到：$$\begin{array}{c}2(\dot{s_i}-\dot{s_1}{)}^{T}u+2(s_i-s_1{)}^T\dot{u}+2r_{i,1}\dot{R_1}+2{\dot{r}}_{i,1}{R}_1\\ =2({\dot{s_i}}^T{s}_i-{\dot{s_1}}^T{s}_1-{\dot{r}}_{i,1}{r}_{i,1})+2d_i\dot{n_{i,1}}+2\dot{d_i}{n}_{i,1}\end{array}.......(3)$$式（2）和式（3）中，$i=1,2,...,M$。对第$i$个传感器与移动目标之间的真实距离$d_i=\Vert u-s_i{\Vert }_2$进行时间求导可以得到：$$\dot{d_i}=(\dot{s_i}-\dot{s_1})(s_i-s_1)/d_i......(4)$$实际应用中，能够得到$M-1$个TDOA和FDOA信息为$T_d=[t_{2,1},t_{3,1},...,t_{M,1}]$和$F_d=[f_{2,1},f_{3,1},...,f_{M,1}]$，其RDOA及其变化率为$r=[r_{2,1},r_{3,1},...,r_{M,1}]$和$\dot{r}=[{\dot{r}}_{2,1},{\dot{r}}_{3,1},...,{\dot{r}}_{M,1}]$之间的关系可以表示为：$$\{\begin{array}{l}r=c\times T_d=d+n\\ \dot{r}=c\times F_d/f_0=\dot{d}+\dot{n}\end{array}$$其中，$c$为信号传输的速度，$n=[n_{2,1},n_{3,1},...,n_{M,1}]$和$\dot{n}=[{\dot{n}}_{2,1},{\dot{n}}_{3,1},...,{\dot{n}}_{M,1}]$分别为相关的噪声向量，并假设噪声为零均值的高斯白噪声，$f_0$为载波频率。

TSWLS算法

第一步估计：First Stage Progressing

对式（2）和式（3）进行合并之后可以得到：$$G_1{\theta }_1-h_1={\epsilon }_1......(5)$$其中，${\theta }_1={[u^T,R_1,{\dot{u}}^T,\dot{R_1}{]}^T}_{8\times 1}$，$$G_1={\left[\begin{array}{cccc}(s_2-s_1{)}^T& r_{2,1}& 0_{1\times 3}& 0\\ ...& ...& ...& ...\\ (s_M-s_1{)}^T& r_{M,1}& 0_{1\times 3}& 0\\ ({\dot{s}}_2-{\dot{s}}_1{)}^T& {\dot{r}}_{2,1}& (s_2-s_1{)}^T& r_{2,1}\\ ...& ...& ...& ...\\ ({\dot{s}}_M-{\dot{s}}_1{)}^T& {\dot{r}}_{M,1}& (s_M-s_1{)}^T& r_{M,1}\end{array}\right]}_{(2M-2)\times 8}$$$$h_1={\left[\begin{array}{c}{s_2}^T{s}_2-{s_1}^T{s}_1-{r_{2,1}}^2\\ ...\\ {s_M}^T{s}_M-{s_1}^T{s}_1-{r_{M,1}}^2\\ {2({\dot{s}}_2}^T{s}_2-{{\dot{s}}_1}^T{s}_1-{\dot{r}}_{2,1}{r}_{2,1})\\ ...\\ 2({{\dot{s}}_M}^T{s}_M-{{\dot{s}}_1}^T{s}_1-{\dot{r}}_{M,1}{r}_{M,1})\end{array}\right]}_{(2M-2)\times 1}$$$${\epsilon }_1=B_1\Delta \eta =\left[\begin{array}{cc}B& 0_{(M-1)\times (M-1)}\\ \dot{B}& B\end{array}\right]\Delta \eta ，\Delta \eta =\left[\begin{array}{c}n\\ \dot{n}\end{array}\right]$$ $$B=2\times diag([d_2,d_3,...,d_M])，\dot{B}=2\times diag([{\dot{d}}_2,{\dot{d}}_3,...,{\dot{d}}_M])$$公式（5）的加权最小二乘(WLS)的估计结果为：$${\theta }_1=(G_1^T{W}_1{G}_1{)}^{-1}{G}_1^T{W}_1{h}_1......(6)$$式中，$W_1=(B_1{Q}^{-1}{B}_1^T{)}^{-1}$，$Q$为量测噪声$\Delta \eta$的协方差矩阵。$$cov({\theta }_1)=(G_1^T{W}_1{G}_1{)}^{-1}......(7)$$

第二步估计：Second Stage Progressing

在TDOA和FDOA算法中，存在两个约束条件：$$\begin{gathered} (u-s_1{)}^T(u-s_1)=R_1^2......(8) \\ (\dot{u}-{\dot{s}}_1{)}^T(u-s_1)={\dot{R}}_1{R}_1......(9) \end{gathered}$$根据式（8）和式（9）可以得到新的约束模型，即：
$$G_2{\theta }_2-h_2={\epsilon }_2......(10)$$式中，令${\theta }_{1,u}=[{\theta }_1(1),{\theta }_1(2),{\theta }_1(3){]}^T$，${\theta }_{1,\dot{u}}=[{\theta }_1(5),{\theta }_1(6),{\theta }_1(7){]}^T$，且有：
$$G_2=\left[\begin{array}{cc}{I}_{3\times 3}& 0_{3\times 3}\\ 1_{1\times 3}& 0_{1\times 3}\\ 0_{3\times 3}& I_{3\times 3}\\ 0_{1\times 3}& 1_{1\times 3}\end{array}\right]，h_2=\left[\begin{array}{c}({\theta }_{1,u}-s_1)\odot ({\theta }_{1,u}-s_1)\\ {\theta }_1(4{)}^2\\ ({\theta }_{1,\dot{u}}-s_1)\odot ({\theta }_{1,u}-s_1)\\ {\theta }_1(8){\theta }_1(4)\end{array}\right]$$$${\theta }_2=\left[\begin{array}{c}(u-s_1)\odot (u-s_1)\\ (\dot{u}-s_1)\odot (u-s_1)\end{array}\right]$$式中，$I_{3\times 3}$为3×3的单位矩阵，$0_{3\times 3}$为3×3的零矩阵，$1_{1\times 3}$为1×3的全为1的矩阵，$0_{1\times 3}$为1×3的全为0的矩阵，$\odot$为运算符（表示两个向量元素和元素乘积，等同于矩阵的点乘）。
公式（10）的加权最小二乘(WLS)最优估计结果为：$${\theta }_2=(G_2^T{W}_2{G}_2{)}^{-1}{G}_2^T{W}_2{h}_2......(11)$$其中，$W_2=(B_{2}cov({\theta }_1)B_2^T{)}^{-1}$，且有：$$B_2=\left[\begin{array}{cccc}2diag(u-s_1)& 0& 0_{3\times 3}& 0\\ 0_{1\times 3}& 2R_1& 0_{1\times 3}& 0\\ diag(\dot{u}-{\dot{s}}_1)& 0& diag(u-s_1)& 0_{3\times 1}\\ 0_{1\times 3}& R_1& 0_{1\times 3}& {\dot{R}}_1\end{array}\right]$$对${\theta }_2$进行估计之后，通过下来公式可以计算移动目标出更优的位置和速度信息：$$\begin{gathered} u=U[\sqrt{{\theta }_2(1)},\sqrt{{\theta }_2(2)},\sqrt{{\theta }_2(3)}{]}^T+s_1......(12) \\ \dot{u}=U[\frac{{\theta }_2(4)}{\sqrt{{\theta }_2(1)}},\frac{{\theta }_2(5)}{\sqrt{{\theta }_2(2)}},\frac{{\theta }_2(6)}{\sqrt{{\theta }_2(3)}}{]}^T+{\dot{s}}_1......(13) \end{gathered}$$式中，$U=diag(sgn({\theta }_{1,u}-s_1))$，$sgn$为正负号判别函数。

综上所述，一直重复迭代计算第一步和第二步，直到两次估计的差值小于预先给定的阈值或者迭代次数足够，则停止迭代并得到最终的最优估计值。下图给出了TSWLS算法的整体流程：

MATLAB程序实现

程序核心函数

function Xk = TwoStepWLS_TDOA_FDOA(Tange, Fange, Base, Noise)
% Ref://Ho K C , Xu W . An accurate algebraic solution for
%     //moving source location using TDOA and FDOA measurements[J].
%     //IEEE Transactions on Signal Processing, 2004, 52(9):2453-2463.
M = size(Base,1)-1;               N = size(Base,2)/2;
%% TDOA&FDOA MODEL(G1 & H1)
G1 = zeros(2*M,2*N+2);            H1 = zeros(2*M,1);
S1 = Base(1, 1:3);                R1 = Base(1, 4:6);
for k = 1: M
    S2 = Base(k+1, 1:N);          R2 = Base(k+1, N+1:end);
    G1(k,:) = 2*[S2-S1, Tange(k), zeros(1,N+1)];
    G1(k+M,:) = 2*[R2-R1, Fange(k), S2-S1, Tange(k)];
    H1(k,1) = (S2*S2' - S1*S1' - Tange(k)*Tange(k));
    H1(k+M,1) = 2*(R2*S2' - R1*S1' - Fange(k)*Tange(k));
end
W1 = inv(Noise);
Xk1 = inv(G1'*W1*G1)*G1'*W1*H1;
clear S1 R1 S2 R2 k Tange Fange W1;
%% First Stage Processing
Times1 = 1;
while Times1 <= 4
    B1 = zeros(M,1);     B2 = zeros(M,1);
    for k = 1: M
        S2 = Xk1(1:N,1) - Base(k+1,1:N)';
        U2 = Xk1(N+2:end-1,1) - Base(k+1,N+1:end)';
        B1(k,1) = 2*norm(S2,2);
        B2(k,1) = 2*S2'*U2/norm(S2,2);
    end
    Bk = [diag(B1), zeros(M,M); diag(B2), diag(B1)];
    W1 = inv(Bk*Noise*Bk');
    Xk1 = inv(G1'*W1*G1)*G1'*W1*H1;
    Times1 = Times1 + 1;
end
Cov1 = inv(G1'*W1*G1);
clear Noise B1 B2 k S2 U2 B1 B2 Bk Times1 W1 G1 H1;
%% Second Stage Processing
U1 = Xk1(1:N);        U2 = Xk1(N+2:2*N+1); 
H2 = [(U1-Base(1,1:N)').^2;    Xk1(N+1)^2;...
    (U2-Base(1,N+1:end)').*(U1-Base(1,1:N)');  Xk1(N+1)*Xk1(end)];
G2 = [eye(N), zeros(N); ones(1,N), zeros(1,N);...
    zeros(N), eye(N); zeros(1,N), ones(1,N)];
Times2 = 1;
while Times2 <= 3
    Bt = diag([U1-Base(1,1:N)'; norm(U1-Base(1,1:N)', 2)]);
    Btd = diag([U2-Base(1,N+1:end)';...
        (U2-Base(1,N+1:end)')'*(U1-Base(1,1:N)')/norm(U1-Base(1,1:N)', 2)]);
    B2 = [2*Bt, zeros(N+1); Btd, Bt];
    W2 = inv(B2*Cov1*B2');
    Xk2 = inv(G2'*W2*G2)*G2'*W2*H2;   
    U1 = diag(sign(U1-Base(1,1:N)'))*sqrt(abs(Xk2(1:N,1))) + Base(1,1:N)';
    U2 = Xk2(N+1:end,1)./(U1-Base(1,1:N)') + Base(1,N+1:end)';
    Times2 = Times2 + 1;
end
clear Xk1 Base M N H2 G2 Times2 Bt Btd B2 W2 Cov1 Xk2;
Xk = [U1;  U2];
end

程序运行主函数

function Result = MainTwoStepWLS(TRange, FRange, Base, sig)
M = size(Base,1) - 1;
R = eye(M, M);
Noise = sig*[R, zeros(M);  zeros(M), 0.1*R];
Result = zeros(size(TRange,1),size(Base,2)+1);
Result(:,1) = TRange(:,1);
for i = 1: size(Result,1)
    Tange = TRange(i,2:end);
    Fange = FRange(i,2:end);
    Xk = TwoStepWLS_TDOA_FDOA(Tange, Fange, Base, Noise);
    Result(i,2:end) = Xk';
end
end

克劳美罗下界CRLB的计算函数

function Ref = CRLB_TDOA_FDOA(True, Base, sig)
% Ref://Ho K C , Xu W . An accurate algebraic solution for
%     //moving source location using TDOA and FDOA measurements[J].
%     //IEEE Transactions on Signal Processing, 2004, 52(9):2453-2463.
% Copyright by Liu Tao. GNSS Research Center of Wuhan University. 27/11/2018.
M = size(Base,1) - 1;     N = size(Base,2)/2;
US1 = True(1,1:N) - Base(1,1:N);
UR1 = True(1,N+1:end) - Base(1,N+1:end);
R1 = norm(US1, 2);  Pus1 = US1/R1;  ER1 = Pus1*UR1';
Q1 = zeros(M,N);  Q2 = zeros(M,N);  Q3 = zeros(M,N);
for k = 1: M
    US2 = True(1,1:N) - Base(k+1, 1:N);
    UR2 = True(1,N+1:end) - Base(k+1, N+1:end);
    R2 = norm(US2,2);  Pus2 = US2/R2;   ER2 = Pus2*UR2';
    Q1(k,:) = Pus2 - Pus1;
    Q3(k,:) = Pus2*ER2/R2 - Pus1*ER1/R1 - UR2/R2 + UR1/R1;
end
Qk = [Q1, Q2; Q3, Q1];
RR = eye(M,M); % 0.5*ones(M, M) + 0.5*eye(M, M);
Noise = sig*[RR, zeros(M);  zeros(M), 0.1*RR];
CRLB = inv(Qk'/Noise*Qk);
Ref = zeros(2,1);
for j = 1:N
    Ref(1) = Ref(1) + CRLB(j,j);
    Ref(2) = Ref(2) + CRLB(j+N,j+N);
end
Ref = sqrt(Ref);
end

仿真实验分析

设置目标的真实位置为$[285,325,275{]}^T$，真实的速度为$[-20,15,40{]}^T$，设置5个传感器的位置和坐标分别如下图所示：
进行10000次模特卡罗仿真，为TDOA量测值和FDOA量测值添加零均值的高斯白噪声，其中TDOA的噪声的方差水平$10log(c^2{\sigma }_d^2)$设置为-20到20，FDOA的噪声的方差为TDOA方差的0.1倍，利用均方根误差RMSE和偏差Bias进行性能分析。

原始数据(TDOA、FDOA)的生成函数

function [TRange, FRange, Base, Source, sig] = SimuTDOA_FDOAData(sig)
% sig为TDOA测量值的噪声的方差
% TRange为TDOA测量值
% FRange为FDOA测量值
% Base为基准站(传感器)的坐标
% Source为真实的目标点的坐标
Base1 = [300, 100, 150, 30, -20, 20];
Base2 = [400, 150, 100, -30, 10, 20];
Base3 = [300, 500, 200, 10, -20, 10];
Base4 = [350, 200, 100, 10, 20, 30];
Base5 = [-100, -100, -100, -20, 10, 10];
Base = [Base1; Base2; Base3; Base4; Base5];
Time = (0.1:0.1:1000)';
Source = [285, 325, 275, -20, 15, 40];
TRange = zeros(size(Time,1), size(Base,1));
TRange(:,1) = Time(:,1);    FRange = TRange;
for i = 1: size(Time,1)
    VAR1 = Source(1,1:3) - Base(1,1:3);
    VAR2 = Source(1,4:6) - Base(1,4:6);
    D1 = norm(VAR1, 2);   R1 = VAR2*VAR1'/D1;
    for j = 1: size(Base,1)-1
        VEC1 = Source(1,1:3) - Base(j+1,1:3);
        VEC2 = Source(1,4:6) - Base(j+1,4:6);
        D2 = norm(VEC1, 2);   R2 = VEC2*VEC1'/D2;
        TRange(i,j+1) = D2 - D1;
        FRange(i,j+1) = R2 - R1;
    end
end
for k = 1: size(TRange,2)
    TRange(:,k) = TRange(:,k) + normrnd(0, sqrt(sig), size(TRange,1), 1);
    FRange(:,k) = FRange(:,k) + normrnd(0, sqrt(0.1*sig), size(FRange,1), 1);
end
end

仿真结果计算和实验分析

function [Bias, RMSE, Ref] = TSWLSResultAnalysis()
close all;clear;clc;
va = (-20:2:20)';
RMSE = zeros(size(va,1),2);
Bias = zeros(size(va,1),2);
Ref = zeros(size(va,1), 2);
for i = 1: size(va,1)
    sig = 10^(va(i)/10);   % Variance of TDOA & FDOA
    [TRange, FRange, Base, Source, sig] = SimuTDOA_FDOAData(sig);
    CRLB = CRLB_TDOA_FDOA(Source, Base, sig);
    Ref(i,1) = CRLB(1);      Ref(i,2) = CRLB(2);
    Result = MainTwoStepWLS(TRange, FRange, Base, sig);
    [B1, R1] = CalculateBiasRMS(Result, Source);
    Bias(i,:) = B1';         RMSE(i,:) = R1';
end
figure();subplot(2,1,1);plot(va, Bias(:,1),'k->','LineWidth',1,'MarkerSize',5,'MarkerFaceColor','k');hold on;
xlabel('10log(\sigma_d^2(m^2))');ylabel('Norm Of Position Bias(m)');
title('Bias of TSWLS for TDOA & FDOA');
subplot(2,1,2);plot(va, Bias(:,2),'k->','LineWidth',1,'MarkerSize',5,'MarkerFaceColor','k');hold on;
xlabel('10log(\sigma_d^2(m^2))');ylabel('Norm Of Velocity Bias(m/s)');

figure();semilogy(va, Ref(:,1),'r->','LineWidth',1,'MarkerSize',5,'MarkerFaceColor','r');hold on;
semilogy(va, RMSE(:,1),'k->','LineWidth',1,'MarkerSize',5,'MarkerFaceColor','k');hold on;
xlabel('10log(\sigma_d^2(m^2))');ylabel('Position RMSE(m)');
legend('CRLB','TSWLS');

figure();semilogy(va, Ref(:,2),'r->','LineWidth',1,'MarkerSize',5,'MarkerFaceColor','r');hold on;
semilogy(va, RMSE(:,2),'k->','LineWidth',1,'MarkerSize',5,'MarkerFaceColor','k');hold on;
xlabel('10log(\sigma_d^2(m^2))');ylabel('Velocity RMSE(m/s)');
legend('CRLB','TSWLS');
end

function [Bias, RMSE] = CalculateBiasRMS(Result,True)
Bias = zeros(2,1);    RMSE = Bias;
s1 = zeros(1,6);  s2 = 0;  s3 = 0;
for i = 1: size(Result,1)
    s1 = s1 + Result(i, 2:7);
    VEC1 = Result(i,2:4) - True(1,1:3);
    VEC2 = Result(i,5:7) - True(1,4:6);
    s2 = s2 + VEC1(1)^2 + VEC1(2)^2 + VEC1(3)^2;
    s3 = s3 + VEC2(1)^2 + VEC2(2)^2 + VEC2(3)^2;
end
s1 = s1/size(Result,1);   VAR = s1 - True;
s2 = s2/size(Result,1);   s3 = s3/size(Result,1);
Bias(1) = sqrt(VAR(1)^2 + VAR(2)^2 + VAR(3)^2);
Bias(2) = sqrt(VAR(4)^2 + VAR(5)^2 + VAR(6)^2);
RMSE(1) = sqrt(s2);       RMSE(2) = sqrt(s3);
end

将上述5个函数文件(.m)放在一个文件夹，在MATLAB窗口中直接运行如下命令即可得到运行后的结果(注: 由于原始数据为随机的，噪声较大时可能会出现矩阵病态的提示)：

[Bias, RMSE, Ref] = TSWLSResultAnalysis();

下面两个图分别给出了TSWLS算法的Bias误差结果以及TSWLS算法的RMSE误差与CRLB下界比较的结果。

TSWLS的均方根误差与CRLB下界的比较

更多的细节可以参见开头的参考文献【完】

更多资料

关于TDOA/FDOA最优算法的研究，可以参考以下三篇文献，也可以去Google Scholar 、ScienceDirect、IEEE Xplore Digital Library、CNKI上查看最新的研究进展和研究成果。
文献1：Yu H, Huang G, Gao J, et al. An efficient constrained weighted least squares algorithm for mov ing source location using TDOA and FDOA measurements[J]. IEEE transactions on wireless communications, 2012, 11(1): 44-47.
文献2：Qu X, Xie L, Tan W. Iterative constrained weighted least squares source localization using TDOA and FDOA measurements[J]. IEEE Transactions on Signal Processing, 2017, 65(15): 3990-4003.
文献3：Noroozi A, Oveis A H, Hosseini S M R, et al. Improved Algebraic Solution for Source Localization From TDOA and FDOA Measurements[J]. IEEE Wireless Communications Letters, 2018, 7(3): 352-355.