satpos.c

GPS卫星位置预报的源程序

#include <math.h>

#define pi 3.1415926535897932


double cursvstate_x,cursvstate_y,cursvstate_z;

void main()
{
	double dtemp;
	//known variables;
	double xn,u,wedot,t,toe,sqra,e;
	double i0,W0,w,M0,idot,WDOT,deltn;
	double Cuc,Cus,Crc,Crs,Cic,Cis;

	double a,n,tk,Mk,Ek,sinvk,cosvk;
	double deltfik,deltrk,deltik,rk;
	double ik,Wk,xp,yp,xs,ys,zs;

	double sinfik,cosfik,sin2fik,cos2fik;

	u=3.986005e+14;
	wedot=7.2921151467e-5;
	
	t=370432.04057965969;
	toe=374400.00000000000;
	sqra=5153.6335582733154;
	e=0.018112989258952439;
	i0=0.31594866281375286720570738080733;
	W0=-0.98084806976839891422150687873562;
	w= 0.26813171803951260567291741853969;
	M0=0.28244944708421824982610687685865;
	idot=-5.9912963479291644894120682387905e-11;
	WDOT=-2.4734845283091999100627108161207e-9;
	deltn=1.147327566286548881581686456882e-9;
	Cuc=-4.0847808122634888e-006;
	Cus=9.5255672931671143e-006;
	Crc=217.37500000000000;
	Crs= -78.406250000000000;
	Cic= -2.1979212760925293e-007;
	Cis= 8.7544322013854980e-008;

	a=sqra*sqra;

	n=sqrt(u/pow(a,3))+deltn;
	tk=t-toe;

	if(tk>302400) tk=tk-604800;
	else if(tk<-302400) tk=tk+604800;
		
	Mk=M0+n*tk;
	xn=Mk;

	do
	{
		dtemp=(xn-e*sin(xn)-Mk)/(1-e*cos(xn));
		xn=xn-dtemp;		
	}while(fabs(dtemp)>1.0e-12);

	Ek=xn;
	sinvk=sqrt(1-e*e)*sin(Ek)/(1-e*cos(Ek));
	cosvk=(cos(Ek)-e)/(1-e*cos(Ek));

	sinfik=sinvk*cos(w)+cosvk*sin(w);
	cosfik=cosvk*cos(w)-sinvk*sin(w);
	sin2fik=2*sinfik*cosfik;
	cos2fik=1-2*pow(sinfik,2);

	deltfik=Cus*sin2fik+Cuc*cos2fik;
	deltrk=Crs*sin2fik+Crc*cos2fik;
	deltik=Cis*sin2fik+Cic*cos2fik;

	rk=a*(1-e*cos(Ek))+deltrk;
	ik=i0+idot*tk+deltik;
	Wk=W0+(WDOT-wedot)*tk-wedot*toe;
	
	xp=rk*(cosfik*cos(deltfik)-sinfik*sin(deltfik));
	yp=rk*(sinfik*cos(deltfik)+cosfik*sin(deltfik));

	xs=xp*cos(Wk)-yp*cos(ik)*sin(Wk);
	ys=xp*sin(Wk)+yp*cos(ik)*cos(Wk);
	zs=yp*sin(ik);
}