MATLAB代做-FPGA代做-GDOP定位算法的MATLAB仿真

發布時間：2022/3/7 瀏覽數：3381

clear,clc

tic

N=1;

R=[1*(pi/180)^2 0;0 1*(pi/180)^2;];

% 3個觀測站的位置S1,S2,S3,目標位置為T
%%%%%%%%% 單位換算成公里!!!
S1=[0 30]*1000;S2=[ 0 -30]*1000;

xt=-41000:2000:41000;
yt=-41000:2000:41000;

S=[S1;S2];
%y1=tan(fe1)*(x-S1(1))+S1(2);%S1的測向射線
%y2=tan(fe2)*(x-S2(1))+S2(2);%S2的測向射線
%y3=tan(fe3)*(x-S3(1))+S3(2);%S3的測向射線
k=1;

for l=1:length(xt)
for j=1:length(yt)

rr1_2=(xt(l)-S(1,1))^2+(yt(j)-S(1,2))^2;
rr2_2=(xt(l)-S(2,1))^2+(yt(j)-S(2,2))^2;
% rr3_2=(xt(l)-S(3,1))^2+(yt(j)-S(3,2))^2;

H(1,1)=-(yt(j)-S(1,2))/rr1_2;H(1,2)=(xt(l)-S(1,1))/rr1_2;
H(2,1)=-(yt(j)-S(2,2))/rr2_2;H(2,2)=(xt(l)-S(2,1))/rr2_2;
% H(3,1)=-(yt(j)-S(3,2))/rr3_2;H(3,2)=(xt(l)-S(3,1))/rr3_2;

Px=pinv(H)*R*pinv(H');

gdop(l,j)=sqrt(Px(1,1)+Px(2,2));
regdop(l,j)=gdop(l,j)/(sqrt(xt(l)*xt(l)+yt(j)*yt(j)));

end
end

Q=[0.5 0.6 0.7 0.8 0.9 1 1.2 1.3 1.5 2 2.5 3 5 7 10];
figure(1);
pic1=contour(xt/1000,yt/1000,gdop'/1000,Q),clabel(pic1);
xlabel('X（Km）');ylabel('Y（Km）');title( 'GDOP(T_1R_2)（Km） ' );hold on ;
plot(S1(1,1)/1000,S1(1,2)/1000,'rP',S2(1,1)/1000,S2(1,2)/1000,'rP');
axis([-40,40,-40,40]);
grid on

W=[0.01 0.02 0.025 0.03 0.035 0.04 0.045 0.05 0.06 0.07 0.08 0.1 0.2 0.5 1 2];

figure(2);
pic2=contour(xt/1000,yt/1000,regdop',W),clabel(pic2);
xlabel('X（Km）');ylabel('Y（Km）');title( '相對GDOP(T_1R_2)' );hold on ;
plot(S1(1,1)/1000,S1(1,2)/1000,'rP',S2(1,1)/1000,S2(1,2)/1000,'rP');
axis([-40,40,-40,40]);
grid on
————————————————