ins3.asv

指北平台惯导导航系统的解算

%*******Files to INS referring to the North-bit platform calculating******
clc;
clear;
%*****
format long g;
DEGRAD=pi/180;
RADDEG=180/pi;
OME=7.29212e-005;            %Earth Rotation rate
Re=6378245.0;                %Radius of Earth
e=1/297;                     %Ellipticity of Earth
dt=0.1;                      %Sampling period
%*********intial settings********
load fw;
fx=f(1,:)';
fy=f(2,:)';
fz=f(3,:)';

Ax=zeros(1,5001)';
Ay=zeros(1,5001)';
Az=zeros(1,5001)';

vx=zeros(1,5001)';
vy=zeros(1,5001)';
vz=zeros(1,5001)';
vy(1)=200;                   %intial speed

lon=zeros(1,5001)';
lat=zeros(1,5001)';
lon(1)=116*DEGRAD;           %intial longitude
lat(1)=39.9*DEGRAD;          %intial latitude
h=500;                       %intial height

%*****calculating*****
rm=Re*(1-2*e+3*e*(sin(lat(1)))^2);
rn=Re*(1+e*(sin(lat(1)))^2);
g=9.7803267714*(1+0.00193185138639*(sin(lat(1)))^2)/sqrt(1-0.0066943799013*(sin(lat(1)))^2);

Ay(1)=fy(1)-vy(1)*vz(1)/rm-(2*OME*sin(lat(1))+vx(1)*tan(lat(1))/rn)*vx(1);
Ax(1)=fx(1)-(2*OME*cos(lat(1))+vx(1)/rn)*vz(1)+(2*OME*sin(lat(1))+vx(1)*tan(lat(1))/rn)*vy(1);
Az(1)=fz(1)-g*(1-2*h/Re)+(2*OME*cos(lat(1)+vx(1)/rn)*vx(1))+vx(1)*vy(1)/rm;

for i=1:5000
g=9.7803267714*(1+0.00193185138639*(sin(lat(i)))^2)/sqrt(1-0.0066943799013*(sin(lat(i)))^2);
rm=Re*(1-2*e+3*e*(sin(lat(i)))^2);
rn=Re*(1+e*(sin(lat(i)))^2);

Ay(i+1)=fy(i+1)-vy(i+1)*vz(i+1)/rm-(2*OME*sin(lat(i+1))+vx(i+1)*tan(lat(i+1))/rn)*vx(i+1);
vy(i+1)=vy(i)+(Ay(i)+Ay(i+1))/2*dt;
lat(i+1)=lat(i)+(vy(i)+vy(i+1))/rm/2*dt;

Ax(i+1)=fx(i+1)-(2*OME*cos(lat(i+1))+vx(i+1)/rn)*vz(i+1)+(2*OME*sin(lat(i+1))+vx(i+1)*tan(lat(i+1))/rn)*vy(i+1);
vx(i+1)=vx(i)+(Ax(i)+Ax(i+1))/2*dt;
lon(i+1)=lon(i)+(vx(i)+vx(i+1))/2/rn*sec(lat(i+1))*dt;

Az(i+1)=fz(i+1)-g*(1-2*h/Re)+(2*OME*cos(lat(i+1)+vx(i+1)/rn)*vx(i+1))+vx(i+1)*vy(i+1)/rm;
vz(i+1)=vz(i)+(Az(i)+Az(i+1))/2*dt;
end

position=[lon,lat]';
speed=[vx,vy]';
save('results','position');%save navigation results

fclose all
close all
%*******printing********
plot(lon.*RADDEG,lat.*RADDEG);
xlabel('longgitude');
ylabel('latitude');
grid on;
hold on;

figure;
subplot(2,2,1),
plot(lon.*RADDEG);
title('longitude');

subplot(2,2,2),
plot(lat.*RADDEG);
title('latitude');

subplot(2,2,3),
plot(vx);
title('vx');

subplot(2,2,4),
plot(vy);
title('vy');