gps正确程序.txt

在给定星历条件下计算卫星坐标

#include<iostream>
#include<fstream>
#include<string>
#include<cmath>

#define GM 3.986004e+14
#define PI 3.1415926
#define ve 7.2921151467e-5

using namespace std;

//GPS时间计算程序
int GetGPSTime(int year,int month,int day,int hour,int minute,double second,double *gpstime,int *weekno)
	{ 
		int dayofw,dayofy,yr,ttlday,m,dinmth[25];
		dinmth[1]=31;dinmth[2]=28;dinmth[3]=31;
		dinmth[4]=30;dinmth[5]=31;dinmth[6]=30;
		dinmth[7]=31;dinmth[8]=31;dinmth[9]=30;
		dinmth[10]=31;dinmth[11]=30;dinmth[12]=31;
		if(year>80) year=year+1900;
		if(year<80) year=year+2000;
		if(year<1981||month<1||month>12||day<1||day>31)
			*weekno=0;
		if(month==1)
			dayofy=day;
		else
		{
			dayofy=0;
			for(m=1;m<=(month-1);m++)
			{
				dayofy+=dinmth[m];
				if(m==2)
				{
					if(year%4==0&&year%100!=0||year%400==0)
						dayofy+=1;}}
			dayofy+=day;}
		ttlday=360;
		for(yr=1981;yr<=(year-1);yr++)
		{
			ttlday+=365;
			if(yr%4==0&&yr%100!=0||yr%400==0)
				ttlday+=1;}
		ttlday+=dayofy;
		*weekno=ttlday/7;
		dayofw=ttlday-7*(*weekno);
		*gpstime=(double)(hour*3600+minute*60+second+dayofw*86400);
		return yr;
    }


int main()
{
        ifstream inFile("E:\\fp1.txt",ios::in);
        ofstream table("E:\\GPS_answer.txt",ios_base::app);

	if(!inFile)
        {
            cout<<"File could not to open!"<<endl;
            exit(1);
        }

	int PRN;
	double s[8][4];


	inFile.ignore(100,'\n');
	inFile.ignore(100,'\n');
	inFile.ignore(100,'\n');
do
{
	inFile>>PRN;
	inFile.ignore(100,'\n');
        //数据转换
	double a0=0.0,x=0.0;
	
	for(int i=1;i<7;i++)
	{
		for(int j=0;j<4;j++)
		{
			inFile>>a0;
			inFile.ignore(1); 
			inFile>>x;
			s[i][j]=a0*pow(10,x);
		}
		inFile.ignore(1);
	}
	inFile.ignore(100,'\n');

	cout<<"PRN="<<PRN<<endl;
    table<<"PRN="<<PRN<<endl;

    double a,gpstime,tk,Mk,Ek,Vk,fk,rk,f0,Ru,Rr,Ri,uk,ik,xk,yk,OMEGAk,Xk,Yk,Zk,s0,s1,s2;
	int weekno,yr,minute;   

    //计算坐标
	double n0,n;
    a=pow(s[2][3],2);
	n0=sqrt(GM/pow(a,3));
	n=n0+s[1][2];          //Delta n=s[1][2]


	 
	
	for(minute=10;minute<=20;minute++)
	{
	yr=GetGPSTime(4,1,30,20,minute,0.0,&gpstime,&weekno);

	//规划时刻
	if(weekno==s[5][2])        // GPS Week=s[5][2]
		tk=gpstime-s[3][0];    //t=gpstime
	                           //t0=s[3][0]
	else return 0;

	Mk=s[1][3]+n*tk;       //M0=s[1][3]
	
	//偏近点角,迭带求解
	double ij=0.0;int count=10;
	for(count=10;count!=0;count--)
	{
		Ek=ij;
		ij=Mk+s[2][1]*sin(Ek);    //e=s[2][1]
	}
	            

	
	 
    // 计算真近点角
	s0=cos(Ek)-s[2][1]; 
    s1=sqrt(1-s[2][1]*s[2][1])*sin(Ek); 
	s2=atan(fabs(s1/s0)); 
    //象限的判别 
    if(s1>0&&s0>0) Vk=s2; 
    if(s1>0&&s0<0) Vk=PI-s2; 
    if(s1<0&&s0<0) Vk=PI+s2; 
    if(s1<0&&s0>0) Vk=2*PI-s2;

    //升交脚距
	fk=Vk+s[4][2];                  //omega=s[4][2] 

	//轨道向径
	rk=a*(1-s[2][1]*cos(Ek));     

	//扰动改正
	f0=fk;
	Ru=s[2][0]*cos(2*f0)+s[2][2]*sin(2*f0);    //升交角距,Cuc=s[2][0],Cus=s[2][2]
	Rr=s[4][1]*cos(2*f0)+s[1][1]*sin(2*f0);    //轨道向径,Crc=s[4][1],Crs=s[1][1]
	Ri=s[3][1]*cos(2*f0)+s[3][3]*sin(2*f0);    //轨道顷角,Cic=s[3][1],Cis=s[3][3]

	uk=fk+Ru;                       //改正后升交角距
	rk=rk+Rr;                       //改正后的轨道向径
	ik=s[4][0]+Ri+s[5][0]*tk;       //i0=s[4][0],IDOT=s[5][0]

	//在轨道坐标系中的坐标
	xk=rk*cos(uk);
	yk=rk*sin(uk);

	//升交点经度
	OMEGAk=s[3][2]+(s[4][3]-ve)*tk-ve*s[3][0];   
	//OMEGA0=s[3][2],OMEGA DOT=s[4][3],t0=s[3][0]


	//在地固坐标系中的坐标
	Xk=xk*cos(OMEGAk)-yk*cos(ik)*sin(OMEGAk);
	Yk=xk*sin(OMEGAk)+yk*cos(ik)*cos(OMEGAk);
	Zk=yk*sin(ik);
	
	cout.precision(30);
	cout<<"x="<<Xk<<"    y="<<Yk<<"     z="<<Zk<<endl;
    table<<"x="<<Xk<<"    y="<<Yk<<"     z="<<Zk<<endl;
	} 
}while(!inFile.eof());
	return 0;
}