DOA/TDOA混合定位CRLB
DOA/TDOA混合观测模型
利用M个观测站进行定位
假设第i个观测站的坐标向量为：
s
i
o
=
[
s
i
x
o
,
s
i
y
o
,
s
i
z
o
]
T
\ \boldsymbol s^o _i =[s^o_{ix},s^o_{iy},s^o_{iz}]^T
sio=[sixo,siyo,sizo]T ，目标的位置向量为：
u
o
=
[
u
x
o
,
u
y
o
,
u
z
o
]
T
\ \boldsymbol u^o =[u^o_{x},u^o_{y},u^o_{z}]^T
uo=[uxo,uyo,uzo]T 。因此，方位角、仰角和TDOA观测量为：
θ
i
o
=
a
r
c
t
a
n
(
u
y
o
−
s
i
,
y
o
u
x
o
−
s
i
,
x
o
)
,
1
≤
i
≤
M
\theta^o_i =arctan \left( \frac{u^o_{y}-s^o_{i,y}} {{u^o_{x}-s^o_{i,x}}} \right),1\leq i \leq M
θio=arctan(uxo−si,xouyo−si,yo),1≤i≤M
ϕ
i
o
=
a
r
c
t
a
n
(
u
z
o
−
s
i
,
z
o
(
u
x
o
−
s
i
,
x
o
)
2
+
(
u
y
o
−
s
i
,
y
o
)
2
)
)
,
1
≤
i
≤
M
\phi ^o_i =arctan \left( \frac{u^o_{z}-s^o_{i,z}} {\sqrt{(u^o_{x}-s^o_{i,x})^2+(u^o_{y}-s^o_{i,y})^2}} ) \right),1\leq i \leq M
ϕio=arctan
(uxo−si,xo)2+(uyo−si,yo)2uzo−si,zo)
,1≤i≤M
τ
j
1
o
=
∣
∣
u
o
−
s
j
o
∣
∣
2
c
−
∣
∣
u
o
−
s
1
o
∣
∣
2
c
,
2
≤
j
≤
M
\tau_{j1}^o= \frac{||\boldsymbol{u}^o-\boldsymbol{s}_j^o||_2}{c}- \frac{||\boldsymbol{u}^o-\boldsymbol{s}_1^o||_2}{c} , 2\leq j \leq M
τj1o=c∣∣uo−sjo∣∣2−c∣∣uo−s1o∣∣2,2≤j≤M
其中
θ
i
o
\theta^o_i
θio ，
ϕ
i
o
\phi ^o_i
ϕio分别表示目标到达第i个观测站的方位角和仰角的真实值，
τ
j
1
o
\tau_{j1}^o
τj1o表示目标到达参考观测站和第j个观测站的时差真实值。由于在实际中我们仅能够获得带有测量噪声的测量值，因此建立如下观测模型：
θ
=
θ
o
+
n
1
=
[
θ
1
o
,
θ
2
o
,
.
.
.
,
θ
M
o
]
T
+
[
δ
θ
1
,
δ
θ
2
,
.
.
.
,
δ
θ
M
]
T
\boldsymbol \theta =\boldsymbol \theta^o+\boldsymbol n_1=[\theta^o_1,\theta^o_2,...,\theta^o_M]^T+[\delta \theta_1,\delta \theta_2,...,\delta \theta_M]^T
θ=θo+n1=[θ1o,θ2o,...,θMo]T+[δθ1,δθ2,...,δθM]T
ϕ
=
ϕ
o
+
n
2
=
[
ϕ
1
o
,
ϕ
2
o
,
.
.
.
,
ϕ
M
o
]
T
+
[
δ
ϕ
1
,
δ
ϕ
2
,
.
.
.
,
δ
ϕ
M
]
T
\boldsymbol \phi =\boldsymbol \phi^o+\boldsymbol n_2 =[\phi ^o_1,\phi ^o_2,...,\phi ^o_M]^T+[\delta \phi _1,\delta \phi _2,...,\delta \phi _M]^T
ϕ=ϕo+n2=[ϕ1o,ϕ2o,...,ϕMo]T+[δϕ1,δϕ2,...,δϕM]T
r
=
r
o
+
n
3
=
[
r
21
o
,
r
31
o
,
.
.
.
,
r
M
1
o
]
T
+
[
δ
r
21
,
δ
r
31
,
.
.
.
,
δ
r
M
1
]
T
\boldsymbol{r}= \boldsymbol{r^o}+\boldsymbol{n}_{3} =[r_{21}^o,r_{31}^o,...,r_{M1}^o]^T +[\delta{r_{21}},\delta{r_{31}},...,\delta{r_{M1}}]^T
r=ro+n3=[r21o,r31o,...,rM1o]T+[δr21,δr31,...,δrM1]T
其中
n
1
\boldsymbol n_1
n1，
n
2
\boldsymbol n_2
n2，
n
3
\boldsymbol n_3
n3的协方差矩阵分别为
Q
1
\boldsymbol{Q}_1
Q1，
Q
2
\boldsymbol{Q}_2
Q2，
Q
3
\boldsymbol{Q}_3
Q3。
因此可以得到DOA/TDOA混合观测向量为：
z
=
z
o
+
n
=
[
(
θ
o
)
T
,
(
ϕ
o
)
T
,
(
r
o
)
T
]
T
+
[
n
1
T
,
n
2
T
,
n
3
T
]
T
\boldsymbol z =\boldsymbol z^o+\boldsymbol n=[(\boldsymbol \theta^o)^T,(\boldsymbol \phi^o)^T,(\boldsymbol{r^o})^T]^T+[\boldsymbol n^T_1,\boldsymbol n^T_2,\boldsymbol n^T_3]^T
z=zo+n=[(θo)T,(ϕo)T,(ro)T]T+[n1T,n2T,n3T]T
其中
n
\boldsymbol{n}
n的协方差矩阵为：
Q
=
b
l
k
d
i
a
g
(
Q
1
,
Q
2
,
Q
3
)
\boldsymbol{Q}=blkdiag({\boldsymbol{Q}_1,\boldsymbol{Q}_2,\boldsymbol{Q}_3})
Q=blkdiag(Q1,Q2,Q3)。
DOA/TDOA混合定位CRLB
位置参数向量为
u
o
=
[
u
x
o
,
u
y
o
,
u
z
o
]
T
\ \boldsymbol u^o =[u^o_{x},u^o_{y},u^o_{z}]^T
uo=[uxo,uyo,uzo]T，测量向量为
z
\boldsymbol z
z。因此
z
\boldsymbol z
z关于
u
o
\ \boldsymbol u^o
uo的对数似然函数为：
l
n
(
p
(
z
∣
u
o
)
)
=
κ
−
(
1
/
2
)
(
z
−
z
o
)
T
Q
−
1
(
z
−
z
o
)
ln(p(\boldsymbol z|\boldsymbol u^o))=\kappa-(1/2)(\boldsymbol z-\boldsymbol z^o)^T\boldsymbol Q^{-1}(\boldsymbol z-\boldsymbol z^o)
ln(p(z∣uo))=κ−(1/2)(z−zo)TQ−1(z−zo)
其中
κ
\kappa
κ表示常数项
因此，Fisher信息矩阵可以表示为：
F
=
E
(
(
∂
l
n
(
p
(
z
∣
u
o
)
)
∂
(
u
o
)
T
)
(
∂
l
n
(
p
(
z
∣
u
o
)
)
∂
(
u
o
)
T
)
T
)
=
(
∂
z
o
∂
(
u
o
)
T
)
Q
−
1
(
∂
z
o
∂
(
u
o
)
T
)
T
F=E \left( \left( \frac {\partial ln(p(\boldsymbol z|\boldsymbol u^o))}{\partial (\boldsymbol u^o)^T} \right) \left( \frac {\partial ln(p(\boldsymbol z|\boldsymbol u^o))}{\partial (\boldsymbol u^o)^T} \right)^T \right)= \left( \frac {\partial \boldsymbol z^o}{\partial (\boldsymbol u^o)^T} \right) \boldsymbol Q^{-1} \left( \frac {\partial \boldsymbol z^o}{\partial (\boldsymbol u^o)^T} \right)^T
F=E((∂(uo)T∂ln(p(z∣uo)))(∂(uo)T∂ln(p(z∣uo)))T)=(∂(uo)T∂zo)Q−1(∂(uo)T∂zo)T
CRLB为Fisher矩阵的逆，因此CRLB表示为：
C
R
L
B
=
(
(
∂
z
o
∂
(
u
o
)
T
)
Q
−
1
(
∂
z
o
∂
(
u
o
)
T
)
T
)
−
1
CRLB=\left( \left( \frac {\partial \boldsymbol z^o}{\partial (\boldsymbol u^o)^T} \right) \boldsymbol Q^{-1} \left( \frac {\partial \boldsymbol z^o}{\partial (\boldsymbol u^o)^T} \right)^T \right)^{-1}
CRLB=((∂(uo)T∂zo)Q−1(∂(uo)T∂zo)T)−1
matlab仿真
clc;
close all;
clear all;
% 测向站数量
M=6;
% 目标数量
N=1;
%观测站位置
s1=[1500 -1900 1400].';
s2=[-1600 2000 1700].';
s3=[1800 -1500 -1600].';
s4=[-1200 1400 -2000].';
s5=[1700 1600 2000].';
s6=[-1800 -1400 -1800].';
for i=1:1:M
str_si=['s',mat2str(i)];
si_value=eval(str_si);
s_true(3*(i-1)+1:3*(i-1)+3,1)=si_value;
end
% 目标位置
u1=[4000 4000 3000].';
for i=1:1:N
str_ui=['u',mat2str(i)];
ui_value=eval(str_ui);
u_true(3*(i-1)+1:3*(i-1)+3,1)=ui_value;
end
%%
angle_noise_power_more=[0.1:0.1:1].*pi/180;
r_noise_power=5;
%% CRLB
for i=1:1:M
eval(['s',mat2str(i),'(:,1)',' = ','s_true(3*(i-1)+1:3*(i-1)+3,1)',';']);
end
for i=1:1:N
eval(['u',mat2str(i),'(:,1)',' = ','u_true(3*(i-1)+1:3*(i-1)+3,1)',';']);
end
%% 对目标位置的导数
% 方位角、仰角对目标位置的导数
diff_theta_ui=zeros(M,3,N);
diff_beta_ui=zeros(M,3,N);
% 距离差对目标位置的导数
diff_r_ui=zeros(M-1,3,N);
for j=1:1:N
for i=1:1:M
str_si=['s',mat2str(i)];
si_value=eval(str_si);
str_ui=['u',mat2str(j)];
ui_value=eval(str_ui);
ui_si=ui_value-si_value;
ai2=sqrt(ui_si(1)^2+ui_si(2)^2);
% 方位角对目标位置的导数
diff_theta_ui_1=-ui_si(2)/ai2^2;
diff_theta_ui_2=ui_si(1)/ai2^2;
diff_theta_ui(i,:,j)=[diff_theta_ui_1,diff_theta_ui_2,0];
% 仰角对目标位置的导数
diff_beta_ui_1=-ui_si(1)*ui_si(3)/ai2/(osjl(ui_value,si_value))^2;
diff_beta_ui_2=-ui_si(2)*ui_si(3)/ai2/(osjl(ui_value,si_value))^2;
diff_beta_ui_3=ai2/(osjl(ui_value,si_value))^2;
diff_beta_ui(i,:,j)=[diff_beta_ui_1,diff_beta_ui_2,diff_beta_ui_3];
end
end
for j=1:1:N
for i=2:1:M
str_si=['s',mat2str(i)];
si_value=eval(str_si);
str_ui=['u',mat2str(j)];
ui_value=eval(str_ui);
ui_si=ui_value-si_value;
% 距离差对目标位置的导数
ui_s1=ui_value-s1;
ui_si=ui_value-si_value;
diff_r_ui(i-1,:,j)=ui_si.'/osjl(ui_value,si_value)-ui_s1.'/osjl(ui_value,s1);
end
end
F_u1=[diff_theta_ui(:,:,1);diff_beta_ui(:,:,1);diff_r_ui(:,:,1);];
F_u=blkdiag(F_u1);
for big_big_loop=1:1:length(angle_noise_power_more)
%方位角
deta_theta=angle_noise_power_more(big_big_loop);
R_theta=deta_theta^2*eye((M),(M));
%仰角
deta_beta=angle_noise_power_more(big_big_loop);
R_beta=deta_beta^2*eye((M),(M));
%距离差
deta_r=r_noise_power;
R_t=(deta_r)^2*(eye(M-1,M-1)+ones(M-1,M-1))/2;
Rn_a1=blkdiag(R_theta,R_beta,R_t);
Rn_a=blkdiag(Rn_a1);
% 观测站定位的先验均方根误差
prior_cov_s(big_big_loop,1)=0;
F1=F_u.'*inv(Rn_a)*F_u;
CRLB1=inv([diff_theta_ui(:,:,1);diff_beta_ui(:,:,1)].'*inv(blkdiag(R_theta,R_beta))*[diff_theta_ui(:,:,1);diff_beta_ui(:,:,1)]);
CRLB2=inv([diff_r_ui(:,:,1)].'*inv(blkdiag(R_t))*[diff_r_ui(:,:,1)]);
CRLB=inv([F1]);
crb_source_DOA_TDOA(big_big_loop,1)=sqrt(trace(CRLB(1:3,1:3)));
CRLB_DOA(big_big_loop,1)=sqrt(trace(CRLB1(1:3,1:3)));
CRLB_TDOA(big_big_loop,1)=sqrt(trace(CRLB2(1:3,1:3)));
end
CRLB_u=[crb_source_DOA_TDOA];
figure(1)
plot(angle_noise_power_more.*180/pi,CRLB_u,'r.-')
hold on;
plot(angle_noise_power_more.*180/pi,CRLB_DOA,'b^-')
hold on;
plot(angle_noise_power_more.*180/pi,CRLB_TDOA,'g*-')
xlabel('角度测量误差')
ylabel('RMSE(m)')
legend('CRLB(DOA/TDOA)','CRLB(DOA)','CRLB(TDOA)')
function [distance]=osjl(object,source)
% 输入均为列向量
distance=sqrt(sum((object-source).^2));
end
clc;
close all;
clear all;
%%
% 测向站数量
M=6;
% 目标数量
N=1;
%观测站位置
s1=[1500 -1900 1400].';
s2=[-1600 2000 1700].';
s3=[1800 -1500 -1600].';
s4=[-1200 1400 -2000].';
s5=[1700 1600 2000].';
s6=[-1800 -1400 -1800].';
for i=1:1:M
str_si=['s',mat2str(i)];
si_value=eval(str_si);
s_true(3*(i-1)+1:3*(i-1)+3,1)=si_value;
end
% 目标位置
u1=[4000 4000 3000].';
for i=1:1:N
str_ui=['u',mat2str(i)];
ui_value=eval(str_ui);
u_true(3*(i-1)+1:3*(i-1)+3,1)=ui_value;
end
%%
angle_noise_power=0.5.*pi/180;
r_noise_power_more=[1:1:10];
%% CRLB
for i=1:1:M
eval(['s',mat2str(i),'(:,1)',' = ','s_true(3*(i-1)+1:3*(i-1)+3,1)',';']);
end
for i=1:1:N
eval(['u',mat2str(i),'(:,1)',' = ','u_true(3*(i-1)+1:3*(i-1)+3,1)',';']);
end
%% 对目标位置的导数
% 方位角、仰角对目标位置的导数
diff_theta_ui=zeros(M,3,N);
diff_beta_ui=zeros(M,3,N);
% 距离差对目标位置的导数
diff_r_ui=zeros(M-1,3,N);
for j=1:1:N
for i=1:1:M
str_si=['s',mat2str(i)];
si_value=eval(str_si);
str_ui=['u',mat2str(j)];
ui_value=eval(str_ui);
ui_si=ui_value-si_value;
ai2=sqrt(ui_si(1)^2+ui_si(2)^2);
% 方位角对目标位置的导数
diff_theta_ui_1=-ui_si(2)/ai2^2;
diff_theta_ui_2=ui_si(1)/ai2^2;
diff_theta_ui(i,:,j)=[diff_theta_ui_1,diff_theta_ui_2,0];
% 仰角对目标位置的导数
diff_beta_ui_1=-ui_si(1)*ui_si(3)/ai2/(osjl(ui_value,si_value))^2;
diff_beta_ui_2=-ui_si(2)*ui_si(3)/ai2/(osjl(ui_value,si_value))^2;
diff_beta_ui_3=ai2/(osjl(ui_value,si_value))^2;
diff_beta_ui(i,:,j)=[diff_beta_ui_1,diff_beta_ui_2,diff_beta_ui_3];
end
end
for j=1:1:N
for i=2:1:M
str_si=['s',mat2str(i)];
si_value=eval(str_si);
str_ui=['u',mat2str(j)];
ui_value=eval(str_ui);
ui_si=ui_value-si_value;
% 距离差对目标位置的导数
ui_s1=ui_value-s1;
ui_si=ui_value-si_value;
diff_r_ui(i-1,:,j)=ui_si.'/osjl(ui_value,si_value)-ui_s1.'/osjl(ui_value,s1);
end
end
F_u1=[diff_theta_ui(:,:,1);diff_beta_ui(:,:,1);diff_r_ui(:,:,1);];
F_u=blkdiag(F_u1);
for big_big_loop=1:1:length(r_noise_power_more)
%方位角
deta_theta=angle_noise_power;
R_theta=deta_theta^2*eye((M),(M));
%仰角
deta_beta=angle_noise_power;
R_beta=deta_beta^2*eye((M),(M));
%距离差
deta_r=r_noise_power_more(big_big_loop);
R_t=(deta_r)^2*(eye(M-1,M-1)+ones(M-1,M-1))/2;
Rn_a1=blkdiag(R_theta,R_beta,R_t);
Rn_a=blkdiag(Rn_a1);
% 观测站定位的先验均方根误差
prior_cov_s(big_big_loop,1)=0;
F1=F_u.'*inv(Rn_a)*F_u;
CRLB1=inv([diff_theta_ui(:,:,1);diff_beta_ui(:,:,1)].'*inv(blkdiag(R_theta,R_beta))*[diff_theta_ui(:,:,1);diff_beta_ui(:,:,1)]);
CRLB2=inv([diff_r_ui(:,:,1)].'*inv(blkdiag(R_t))*[diff_r_ui(:,:,1)]);
CRLB=inv([F1]);
crb_source_DOA_TDOA(big_big_loop,1)=sqrt(trace(CRLB(1:3,1:3)));
CRLB_DOA(big_big_loop,1)=sqrt(trace(CRLB1(1:3,1:3)));
CRLB_TDOA(big_big_loop,1)=sqrt(trace(CRLB2(1:3,1:3)));
end
CRLB_u=[crb_source_DOA_TDOA];
%%
figure(1)
plot(r_noise_power_more,CRLB_u,'r.-')
hold on;
plot(r_noise_power_more,CRLB_DOA,'b^-')
hold on;
plot(r_noise_power_more,CRLB_TDOA,'g*-')
xlabel('TDOA测量误差')
ylabel('RMSE(m)')
legend('CRLB(DOA/TDOA)','CRLB(DOA)','CRLB(TDOA)')
%%
%%
function [distance]=osjl(object,source)
% 输入均为列向量
distance=sqrt(sum((object-source).^2));
end
扫一扫
8412
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
