clear
clc
format long
tp=input('tp=');
toc=input('toc=');
a0=input('a0=');
a1=input('a1=');
a2=input('a2=');
toe=input('toe=');
M0=input('M0=');
a=input('长半径a=');
deltan=input('卫星平均角速度之差deltan=');
e=input('e=');
w=input('w=');
Cuc=input('Cuc=');
Cus=input('Cus=');
Cic=input('Cic=');
Cis=input('Cis=');
Crc=input('Crc=');
Crs=input('Crs=');
i0=input('i0=');
I=input('轨道倾角变化率I=');
OM0=input('OM0=');
OM=input('升交点赤径变化率OM=');
tt=a0+a1*(tp-toc)+a2*(tp-toc);
t=tp-tt;
tk=t-toe;
u=3986005*10^8;
n0=sqrt(u/a^3);
n=n0+deltan;
Mk=M0+n*tk;
Dk=1;
Ek=0;
n1=0;
while abs(Ek-Dk)>0.0000000001
    n1=n1+1;
    Ek=Dk;
    Dk=Mk+e*sin(Ek);
end
Ek=Dk;
Vk=atan(sqrt(1-e*e)*sin(Ek)/(cos(Ek)-e));
if sin(Ek)>0 & cos(Ek)-e<0
    Vk=pi-Vk;
elseif sin(Ek)<0 & cos(Ek)-e<0
    Vk=pi+Vk;
elseif sin(Ek)<0 & cos(Ek)-e>0
    Vk=2*pi-Vk;
end
Faik=Vk+w;
SigmaU=Cuc*cos(2*Faik)+Cus*sin(2*Faik);
SigmaR=Crc*cos(2*Faik)+Crs*sin(2*Faik);
SigmaI=Cic*cos(2*Faik)+Cis*sin(2*Faik);
Uk=Faik+SigmaU;
Rk=a*(1-e*cos(Ek))+SigmaR;
Ik=i0+SigmaI+I*tk;
X0=Rk*cos(Uk);
Y0=Rk*sin(Uk);
we=7.29211567*10^(-5);
OMK=OM0+(OM-we)*tk-we*toe;
Xk=X0*cos(OMK)-Y0*cos(Ik)*sin(OMK);
Yk=X0*sin(OMK)+Y0*cos(Ik)*cos(OMK);
Zk=Y0*sin(Ik);
disp(['卫星钟差改正dt=',num2str(tt)])
disp(['归化时刻tk=',num2str(tk)])
disp(['平均运行角速度n=',num2str(n)])
disp(['卫星平近点角Mk=',num2str(Mk)])
disp(['偏近点角Ek=',num2str(Ek)])
disp(['真近点角Vk=',num2str(Vk)])
disp(['升交距角Faik=',num2str(Faik)])
disp(['摄动改正项SigmaU=',num2str(SigmaU)])
disp(['摄动改正项SigmaR=',num2str(SigmaR)])
disp(['摄动改正项SigmaI=',num2str(SigmaI)])
disp('经过摄动改正项：')
disp(['升交距角Uk=',num2str(Uk)])
disp(['卫星矢径Rk=',num2str(Rk)])
disp(['轨道倾角Ik=',num2str(Ik)])
disp('卫星在轨道平面坐标系的坐标')
disp(['X0=',num2str(X0)])
disp(['Y0=',num2str(Y0)])
disp(['观测时刻升交点经度OMK=',num2str(OMK)])
disp('卫星在地心固定坐标系中的直角坐标')
disp(['Xk=',num2str(Xk)])
disp(['Yk=',num2str(Yk)])
disp(['Zk=',num2str(Zk)])