%% 条件
% $s1=(-10,0,0)km, s2=(10,0,0)km T=(x,y,10)km, \sigma_{\beta}=1mrad,\sigma_{\varepsilon}=1mrad,\sigma_s = 0.001km$
clear all; close all; clc;
ang_1 = 0.001; %方位角标准差
ang_2 = 0.001; %俯仰角标准差
dx = 0.001; %站址误差
stations = [-10,0,0;10,0,0]; %基站位置
%% 在([-100,100][-100,100])遍历目标点（Z = 10Km），绘制不同目标点的GPOD
X = [-100:1:100]; % km
Y = [-100:1:-20,20:1:100];
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),10],ang_1,ang_2,dx);
end
end
G = real(G);
figure;
mesh(X,Y,G');hold on;grid on;
plot3(stations(:,1),stations(:,2),[0,0],'o','Linewidth',1.5);
xlabel('x(km)');ylabel('y(km)');zlabel('z(km)');
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_1(-10,0,0),S_2(10,0,0),\sigma_{\beta}=1mrad,sigma_{\varepsilon}=1mrad,\sigma_s = 0.001km $$}','Interpreter','latex');
axis equal;
axis tight;
%% 取出Y方向的几个特殊列，观察定位误差与X方向距离的关系
figure(3);
plot(X,G(:,[82,102,142]));
legend('Y=20km','Y=40km','Y=80km');
%% 计算GPOD的过程
function [gdop] = gdop_1 (stations,t,ang_1,ang_2,dx)
x1 = stations(1,1);
y1 = stations(1,2);
z1 = stations(1,3);
x2 = stations(2,1);
y2= stations(2,2);
z2 = stations(2,3);
x = t(1);
y = t(2);
z = t(3);
F = zeros(3);
Px = zeros(3);
Pdr = zeros(3);
Pdq = [ang_1^2,0,0;0,ang_1^2,0;0,0,ang_2^2];
L = sqrt((x-x1)^2+(y-y1)^2+(z-z1)^2);
d1 = (x-x1)^2+(y-y1)^2;
d2 = (x-x2)^2+(y-y2)^2;
m1 = (x-x1)*(y-y1)*(z-z1);
N = L^2*sqrt(d1);
F(1,1) = -(y-y1)/d1;
F(1,2) = -(x-x1)/d1;
F(1,3) = 0;
F(2,1) = -(y-y2)/d2;
F(2,2) = -(x-x2)/d2;
F(2,3) = 0;
F(3,1) = -((x-x1)*(z-z1))/(L^2*sqrt(d1));
F(3,2) = -((y-y1)*(z-z1))/(L^2*sqrt(d1));
F(3,3) = sqrt(d1)/(L^2);
Px(1,1) = 1/d1;
Px(1,2) = 0;
Px(1,3) = 2*m1/(d1*N);
Px(2,1) = 0;
Px(2,2) = 1/d2;
Px(2,3) =0;
Px(3,1) = Px(1,3);
Px(3,2) = 0;
Px(3,3) = d1*(z-z1)^2/(N^2)+N^2/L^8;
Px = dx.*Px;
Pdr = (F'*F)^(-1)*(F')*(Pdq+Px)*F*((F'*F)^(-1));
gdop =sqrt(trace(Pdr));
end
评论10