%由于编程水平限制，本程序执行时间较长，请老师耐心等待
clear;clc;%清除内存变量，清理命令窗口
format long %改变数字的显示格式，使数字显示为长格式
yz=load('fw.mat');
f=yz.f;
a=length(f(:,1)) ;
b=length(f(1,:));
Vx=ones(1,a);%定义一个1*a列的矩阵用来存储东向速度信息
Vy=ones(1,a);%定义一个1*a列的矩阵用来存储北向速度信息
Vx(1,1)=0; %东向加速度初始值
Vy(1,1)=0;%北向加速度初始值
L=ones(1,b);%定义一个1*a列的矩阵用来存储纬度信息
V=ones(1,b);%定义一个1*a列的矩阵用来存储经度信息
L(1,1)=39.981430918136*pi/180 ;%纬度初始值
V(1,1)=116.344762072818*pi/180 ;%经度初始值
h=40.8236;%载体高度
e=1/298.3; %椭圆度e的值
Re=6378245 ;%地球长半径Re的值,单位为米
wie=2*pi/(24*3600);%求解地球自转角速度
dt=0.01;
for(k=1:b)%定义循环变量k从1到b，以1为步长
Rn=Re*(1+e*sin(L(1,k))^2);%卯酉圈主曲率半径
R=1/(Rn+h);
ax=f(1,k)+2*wie*sin(L(1,k))*Vy(1,k)+R*tan(L(1,k))*Vx(1,k)*Vy(1,k) ; %求解东向速度的增量
ay=f(1,k)-2*wie*sin(L(1,k))*Vx(1,k)+R*tan(L(1,k))*Vx(1,k) ; %求解北向速度的增量
Vx(1,k+1)=Vx(1,k)+ax*dt; %求东向速度
Vy(1,k+1)=Vy(1,k)+ay*dt; %求北向速度
dL=Vy(1,k+1)*(1+2*e-3*e*sin(L(1,k))^2)/Re;%求纬度增量
dV=Vy(1,k+1)*(1-e*sin(L(1,k))^2)/(Re*cos(L(1,k)));%求经度增量
L(1,k+1)=L(1,k)+dL ;%求纬度
V(1,k+1)=V(1,k)+dV;%求经度
end %循环结构结束
L=360*L/(2*pi);%纬度值用度数表示
V=360*V/(2*pi);%经度值用度数表示
plot(V(1,:),L(1,:)) %以经度为横轴，纬度为纵轴作系统位置坐标曲线
grid %加注坐标网络
ylabel('纬度（°）')%标记横坐标周围经度
xlabel('经度（°）')%标记纵坐标轴为纬度
hold on
plot(V(1,1),L(1,1),'r*')%在起始点出画一红色的*点
title('系统位置坐标曲线图')%标记图名
disp('系统纬度终点值')
disp(L(1,b))%纬度终点值
disp('系统经度终点值')
disp(V(1,b))%经度终点值
disp('系统东向速度终点值')
disp(Vx(1,b))%系统东向速度终点值
disp('系统北向速度终点值')
disp(Vy(1,b))%系统北向速度终点值
