- 更新:由于站址排布不同时,定位性能不同,有时会出现部分区域不可定位的现象,表现为GDOP特别大,造成可视化效果不好。在代码中,将GDOP过分大的点,视为不可定位点,将其置零,可解决问题。代码和效果如下:
在上篇文章中,我们详细推导了TDOA的定位几何精度,下面给出使用MATLAB软件将其可视化的代码。
条件:已知站址,目标高度,时间差误差的方差,站址误差的方差。
菱形布站:
平行四边形布站:
正方形布站:
Y形布站:
立体图:
等高线:
取出等高线中若干条:
%% 条件 s0=(0,0,0) s1 = (-20,20,0) s2 = (20,20,0) s3 = (0,-20,0) H = 10km, td = 5ns, ts = 0.005km ,相关系数0
clear all; close all; clc;
td = 5*1e-9; %测时差误差
ts = 0.005; %站址误差
stations = [-10 10 0.3;10 -10 0.2;-10 -10 0.3;10 10 0.2]; %基站位置
%% 在([-100,100][-100,100])遍历目标点(Z = 10Km),绘制不同目标点的GPOD
X = [-200:1:200]; % km
Y = [-200:1:200];
G=zeros(length(X),length(Y),'double');
for i = 1:length(X)
for j = 1:length(Y)
G(i,j) = gdop_1(stations,[X(i),Y(j),15],td,ts);
end
end
G = real(G);
%更新的代码:
G(G>15)=0; %大于15Km视为定位失败,剔除数据,可以自己定义这个阈值
%更新的代码
figure;
mesh(X,Y,G');hold on;grid on;
plot3(stations(:,1),stations(:,2),stations(:,3),'o','Linewidth',1.5);
xlabel('x(km)');ylabel('y(km)');zlabel('z(km)');
title('确定站址,时间差测量误差,站址误差后,定位误差与目标所在位置的关系')
%% 以等高线呈现
figure;
[C,h] = contour(X,Y,G',15);hold on; grid on; %contour绘制等高线
clabel(C,h); %clabel为等高线添加标签
plot(stations(:,1),stations(:,2),'o');
xlabel('x(km)');ylabel('y(km)');
title('{$$ S_0=(0,0,0) ,S_1 = (-20,20,0) S_2 = (20,20,0), S_3 = (0,-20,0) H = 10Km, t_d = 5ns, t_s = 0.005Km ,\eta =0 $$}','Interpreter','latex');
axis equal;
axis tight;
%% 取出Y方向的几个特殊列,观察定位误差与X方向距离的关系
figure(3);
plot(X,G(:,[121,141,181]));
legend('Y=20km','Y=40km','Y=80km');
title('定位误差与X方向的关系')
xlabel('x(km)');ylabel('y(km)');
%% 计算GPOD的过程
function [gdop] = gdop_1 (stations,t,td,ts)
x1 = stations(1,1);
y1 = stations(1,2);
z1 = stations(1,3);
x2 = stations(2,1);
y2= stations(2,2);
z2 = stations(2,3);
x3 = stations(3,1);
y3= stations(3,2);
z3 = stations(3,3);
x4 = stations(4,1);
y4= stations(4,2);
z4 = stations(4,3);
x = t(1);
y = t(2);
z = t(3);
r1 = sqrt((x-x1)^2+(y-y1)^2+(z-z1)^2);
r2 = sqrt((x-x2)^2+(y-y2)^2+(z-z2)^2);
r3 = sqrt((x-x3)^2+(y-y3)^2+(z-z3)^2);
r4 = sqrt((x-x4)^2+(y-y4)^2+(z-z4)^2);
c1x = (x-x1)/r1;c2x = (x-x2)/r2;c3x = (x-x3)/r3;c4x = (x-x4)/r4;
c1y = (y-y1)/r1;c2y = (y-y2)/r2;c3y = (y-y3)/r3;c4y = (y-y4)/r4;
c1z = (z-z1)/r1;c2z = (z-z2)/r2;c3z = (z-z3)/r3;c4z = (z-z4)/r4;
F=[c1x-c2x,c1y-c2y,c1z-c2z;c1x-c3x,c1y-c3y,c1z-c3z;c1x-c4x,c1y-c4y,c1z-c4z];
Px = [2*ts^2,ts^2,ts^2;ts^2,2*ts^2,ts^2;ts^2,ts^2,2*ts^2];
Pdq = [td^2,0,0;0,td^2,0;0,0,td^2];
Pdr = (F'*F)^(-1)*(F')*(Pdq+Px)*F*((F'*F)^(-1));
gdop =sqrt(trace(Pdr));
end