TDOA定位CRLB

TDOA定位的CRLB

TDOA观测模型

在三维直角坐标系中，考虑利用M个观测站对一个目标进行定位。
第i个观测站位置向量记为 ${\mathbf{s}}_i^o=[s_{x,i}^o,s_{y,i}^o,s_{z,i}^o{]}^T,1\le i\le M$
目标位置向量记为${\mathbf{u}}^o=[u_x^o,u_y^o,u_z^o{]}^T$

将第一个观测站作为TDOA测量的参考观测站，则目标到达其余观测站和参考观测站之间的TDOA可以表示为：
$${\tau }_{j1}^o=\frac{||{\mathbf{u}}^o-{\mathbf{s}}_j^o|{|}_2}{c}-\frac{||{\mathbf{u}}^o-{\mathbf{s}}_1^o|{|}_2}{c},2\le j\le M$$
其中c表示信号传播速度，通常是已知的，因此TDOA可以转换为到达距离差（Range Difference Of Arrival，RDOA）：
$$r_{j1}^o=||{\mathbf{u}}^o-{\mathbf{s}}_j^o|{|}_2-||{\mathbf{u}}^o-{\mathbf{s}}_1^o|{|}_2,2\le j\le M$$
因此建立RDOA观测模型为：
$$\mathbf{r}={\mathbf{r}}^{\mathbf{o}}+{\mathbf{n}}_{\mathbf{r}}=[r_{21}^o,r_{31}^o,...,r_{M1}^o{]}^T+[\delta r_{21},\delta r_{31},...,\delta r_{M1}{]}^T$$
${\mathbf{n}}_{\mathbf{r}}$表示测量噪声向量，协方差矩阵表示为${\mathbf{Q}}_{\mathbf{r}}$。

TDOA定位CRLB

未知参数向量为${\mathbf{u}}^{\mathbf{o}}=[u_x^o,u_y^o,u_z^o{]}^T$，测量向量为$\mathbf{r}$。因此$\mathbf{r}$关于${\mathbf{u}}^{\mathbf{o}}$的对数似然函数为：
$$ln(p(\mathbf{r}|{\mathbf{u}}^{\mathbf{o}}))=\kappa -(1/2)(\mathbf{r}-{\mathbf{r}}^o{)}^T{\mathbf{Q}}_{\mathbf{r}}^{-1}(\mathbf{r}-{\mathbf{r}}^o)$$
其中$\kappa$表示常数项

则CRLB表示为：
$$CRLB={\left(\left(\frac{\partial {\mathbf{r}}^o}{\partial ({\mathbf{u}}^{\mathbf{o}}{)}^T}\right){\mathbf{Q}}_{\mathbf{r}}^{-1}{\left(\frac{\partial {\mathbf{r}}^o}{\partial ({\mathbf{u}}^{\mathbf{o}}{)}^T}\right)}^T\right)}^{-1}$$

matlab仿真

clc;
close all;
clear all;
%%
% 测向站数量
M=6;
% 目标数量
N=1;
% 6个测向站的位置坐标,单位米
% s1=[300 -100 150];
% s2=[-400 150 200];
% s3=[300 500 -300];
% s4=[350 200 100];
% s5=[-100 -100 -100];
% s6=[200 -300 -200];
s1=[1200 1800 200];
s2=[-1500 -800 150];
s3=[1400 -600 -200];
s4=[-800 1200 120];
s5=[1300 -800 -250];
s6=[-1000 1600 -150];
% 目标位置,单位米
s_all=[s1;s2;s3;s4;s5;s6];

u=[2200 1800 2000].';
u=[8000 6800 3000].';
%%
ri0=zeros(M,1);
for i=1:1:M
    ri0(i,1)=osjl(u,s_all(i,:).');
end
rij0=ri0-ri0(1);
%%
Gt0=zeros(M-1,4);
ht0=zeros(M-1,1);
for i=2:1:M
    Gt0(i-1,:)=-1*[(s_all(i,:)-s_all(1,:)),rij0(i,1)];
    ht0(i-1,1)=0.5*((rij0(i,1))^2-s_all(i,:)*s_all(i,:).'+s_all(1,:)*s_all(1,:).');
end
result11=ht0-Gt0*[u;ri0(1)];
%%
% 距离差对目标位置的导数
r_diff_u=zeros(M-1,3);
for i=2:1:M
    % 距离差对目标位置的导数
    ui_s1=u.'-s_all(1,:);
    ui_si=u.'-s_all(i,:);
    r_diff_u(i-1,:)=ui_si/osjl(u.',s_all(i,:))-ui_s1/osjl(u.',s_all(1,:));
end
%%
r_noise_power=0.2:0.2:2;
big_loop_number=length(r_noise_power);
iter_number=zeros(big_loop_number,small_loop_number);
%%
start_time=clock;
for big_loop=1:1:big_loop_number
    cov_r=r_noise_power(big_loop)^2*(eye(M-1)+ones(M-1,M-1))/2;
    %%
    F=r_diff_u.'*inv(cov_r)*r_diff_u;
    CRLB_u(big_loop)=sqrt(trace(inv(F)));
end
figure(1)
plot(r_noise_power,CRLB_u,'r.-')
xlabel('TDOA噪声(m)')
ylabel('RMSE(m)')

%%
function [distance]=osjl(object,source)
% 输入均为列向量
distance=sqrt(sum((object-source).^2));
end