精密星历计算卫星位置

//sp3(p类型)，拉格朗日多项式九阶内插法 //本程序并未使用前一天和后一天的数据进行插值运算,故子时时段的数据精度低于其他时段精度 //另：本程序中卫星的PRN号默认为从G01开始，并未从sp3文件中读取，故使用本程序时应注意检验文件中对应的PRN号 #include <iostream> #include <fstream> #include <string> #include <stdlib.h> #define N 10 using namespace std; double lagrange(int x[N],double y[N],int xx) { int n=9; double p,yy; int i,k; yy = 0.0; for(k=0; k<n; k++) { p=1.0; for(i=0; i<n; i++) if(i!=k) { p=p*(xx-x[i])/(x[k]-x[i]); } yy+=(p*y[k]); } return yy; } int main() { char name[100]; cout<<"输入sp3文件名:"; cin>>name; string line; fstream iofile(name,ios::in|ios::out|ios::binary); if(!iofile) { cerr<<"open error!"<<endl; exit(1); } int y,mou,d,h,m,s;int math;//年月日时分秒 cout<<"输入查询的年月日时分秒（如:2010 1 3 0 0 0）"<<endl; cin>>y>>mou>>d>>h>>m>>s; int t=h*3600+m*60+s; for(int i = 0; i < 2; i++)//取第n行数据，则i<n-1 { getline(iofile, line); } getline(iofile, line); double num[96][100][3];//一天96组观测数据，至多一百颗卫星的xyz值 basic_string<char>line1=line.substr(4,2); int sat = atoi( line1.c_str() );//sat为卫星数 for(int i = 0; i < 19; i++)//取第n行数据，则i<n-4 { getline(iofile, line); } int year[96],mouth[96],day[96],hour[96],min[96]; basic_string<char>line2,line3,line4,line5; for(int h=0;h<96;h++)//读取历元的年月日 { getline(iofile, line); line1=line.substr(3,4); year[h] = atoi( line1.c_str() ); line2=line.substr(8,2); mouth[h] = atoi(line2.c_str()); line3=line.substr(11,2); day[h] = atoi(line3.c_str()); line4=line.substr(14,2); hour[h] = atoi(line4.c_str()); line5=line.substr(17,2); min[h] = atoi(line5.c_str()); for(int g=0;g<sat;g++)//读取历元的各个卫星位置 { getline(iofile, line); line1=line.substr(4,14); num[h][g][0] = atof( line1.c_str() ); line2=line.substr(18,14); num[h][g][1] = atof( line2.c_str() ); line3=line.substr(32,14); num[h][g][2] = atof( line3.c_str() ); }} if(y!=year[0]||mou!=mouth[0]||d!=day[0]) { cerr<<"date error!"<<endl; exit(1); } int gps[96]; for(int w=0;w<96;w++) gps[w]=hour[w]*3600+min[w]*60; int time[9];double xx[9],yy[9],zz[9]; if(t<=4500) {math=0; for(int i=0;i<9;i++) {time[i]=gps[i];}} else if(t>=81900) { math=86; for(int i=0;i<9;i++) {time[i]=gps[i+86];}} else for(int i=0,count=0;i<96;i++) { if(t>gps[i]) {count++;} else for(int j=0;j<9;j++) time[j]=gps[count-4+j]; math=count-4; } double xxx[9],yyy[9],zzz[9]; for(int j=0,coun=1;j<sat;j++,coun++){ for(int m=math,i=0;m<math+9;m++,i++) {xxx[i]=num[m][j][0]; yyy[i]=num[m][j][1]; zzz[i]=num[m][j][2];} //cout<<time[i]<<" "<<yyy[i]<<" "<<zzz[i]<<endl; double a=lagrange(time,xxx,t); double b=lagrange(time,yyy,t); double c=lagrange(time,zzz,t); cout<<"P"<<"G0"<<coun<<" "<<a<<" "<<b<<" "<<c<<endl; } iofile.close(); return 0; }