⭐ 欢迎来到虫虫下载站！ | 📦 资源下载 📁 资源专辑 ℹ️ 关于我们
⭐ 虫虫下载站
📤 上传资源

📄 sp3.cpp

📁 GPS 定位授时源码
💻 CPP
字号:
🔍 

#include "SP3.h"
#include "StdAfx.h"
#include "GlobleDefine.h"
#include "fstream.h"
#include "Matrixcsl.h"
#include "TimeCorTran.h"
#include "math.h"

/*void VECTORLAGRANGE(long double x, long double *xi, long double **yi,int iTimeOrderN,long double *y,int iDim )
{
int i,j,k,p;
long double dCofficient;//插值求待求点值
long double dDerivaBase;//插值求待求点导数
long double dDerivaIni;
for (k=0;k<2*iDim;k++)
	{
		y[k]=0;
	}

//const int N =17;
for(i=0;i<iTimeOrderN;i++)
{
	dCofficient =1.0;
	dDerivaBase =1.0;
	dDerivaIni  =0.0;
	for(j=0;j<iTimeOrderN; j++)
	{	if(j!=i)
		{	
			dCofficient=dCofficient*(x-xi[j])/(xi[i]-xi[j]);
			dDerivaBase=dDerivaBase*10000/(xi[i]-xi[j]);
		}
		else continue;
	}
    long double lRem=dDerivaBase/10000;// 	for(j=0; j<iTimeOrderN; j++)
	{	
		if(j!=i)
		{	
			dDerivaBase=1.0;
			for(p=0;p<iTimeOrderN;p++)
			{	
				if((p!=i)&&(p!=j))
				{	
					
					dDerivaBase=dDerivaBase *(x-xi[p])/10000;
				}
				else continue ;
			}	
			dDerivaIni+=dDerivaBase;
		}
		else continue;
	}

	for (k=0;k<iDim;k++)
	{
		y[k]=y[k]+yi[i][k]*dCofficient;
	}
	for (k=iDim;k<2*iDim;k++)
	{
		y[k]=y[k]+yi[i][k-iDim]*dDerivaIni*lRem;
	}
	
}
}
*/

void NEWTONCOEF(long double *xi, long double **yi,long double **coef,int iPolyOrder,int iDim )
{
	int i,j,k,p;	
	long double *lBase =new long double[iPolyOrder];	
	

	for(j=0;j<iDim;j++)
	{	coef[0][j]=yi[0][j];
		for(i=1;i<iPolyOrder;i++)	
		{	
			for(p=0;p<iPolyOrder;p++)
				lBase[p]=1;
			long double lSum=0;
			for(k=0;k<i;k++)
			{	lBase[i]*=(xi[i]-xi[k]);
				if((k+1)<i)
					lSum+=(coef[k+1][j]*lBase[i]);
			}
			coef[i][j]=(yi[i][j]-yi[0][j]-lSum)/lBase[i];
		}
	}
}

void CREATSP3POLYCOEF(int iIni, int iPolyOrder,int iSatPrn,long double **coef,NAVSP3EPO *RecordEpo)
{
	long double *xi;
	long double **yi;
	xi=new long double[iPolyOrder];
	yi=TwoArrayAlloc(iPolyOrder,3);
	GPSTIME time;
	UTCCOVGPS(&RecordEpo[iIni].EpoTime,&time);
	for (int i=0;i<iPolyOrder;i++)
	{			
		xi[i]=time.dWEEKSECONDS+900*i;
		yi[i][0]=RecordEpo[i+iIni].NavSp3[iSatPrn-1].xyzt.xyz.lX;
		yi[i][1]=RecordEpo[i+iIni].NavSp3[iSatPrn-1].xyzt.xyz.lY;
		yi[i][2]=RecordEpo[i+iIni].NavSp3[iSatPrn-1].xyzt.xyz.lZ;
		//yi[i][3]=RecordEpo[i+iIni].NavSp3[iSatPrn-1].xyzt.lT;
	}


	NEWTONCOEF(xi,yi,coef,iPolyOrder,3);
	delete[]xi;
	TwoArrayFree(yi);
}



void CREATCLKPOLYCOEF(int iIni, int iPolyOrder,int iSatPrn,long double **coef,NAVSP3EPO *RecordEpo)
{
	long double *xi;
	long double **yi;
	xi=new long double[iPolyOrder];
	yi=TwoArrayAlloc(iPolyOrder,1);
	GPSTIME time;
	UTCCOVGPS(&RecordEpo[iIni].EpoTime,&time);
	for (int i=0;i<iPolyOrder;i++)
	{			
		xi[i]=time.dWEEKSECONDS+900*i;
		//yi[i][0]=RecordEpo[i+iIni].NavSp3[iSatPrn-1].xyzt.xyz.lX;
		//yi[i][1]=RecordEpo[i+iIni].NavSp3[iSatPrn-1].xyzt.xyz.lY;
		//yi[i][2]=RecordEpo[i+iIni].NavSp3[iSatPrn-1].xyzt.xyz.lZ;
		if(RecordEpo[i+iIni].NavSp3[iSatPrn-1].xyzt.lT>10000.0)
		{	if(RecordEpo[i+iIni-1].NavSp3[iSatPrn-1].xyzt.lT>10000.0)
				RecordEpo[i+iIni].NavSp3[iSatPrn-1].xyzt.lT=RecordEpo[i+iIni-2].NavSp3[iSatPrn-1].xyzt.lT;
			else RecordEpo[i+iIni].NavSp3[iSatPrn-1].xyzt.lT=RecordEpo[i+iIni-1].NavSp3[iSatPrn-1].xyzt.lT;
		}
		yi[i][0]=RecordEpo[i+iIni].NavSp3[iSatPrn-1].xyzt.lT;

	}
	
	
	NEWTONCOEF(xi,yi,coef,iPolyOrder,1);
	delete[]xi;
	TwoArrayFree(yi);
}

void READNAVSP3HEAD(ifstream InFile,ofstream OutFile,NAVSP3HEAD &head)
{
	char str[81];
	CString s;
	
		InFile.getline(str,81,'\n');
		s=str;
		head.CPorV              = str[2];
		head.FirstTime.iYEAR    = atoi(s.Mid(3,5));
		head.FirstTime.iMONTH   = atoi(s.Mid(8));
		head.FirstTime.iDAY     = atoi(s.Mid(11));
		head.FirstTime.iHOUR    = atoi(s.Mid(14));
		head.FirstTime.iMINUTE  = atoi(s.Mid(17));
		head.FirstTime.dSECOND  = atof(s.Mid(20));
		head.iEpoNumber         = atoi(s.Mid(32));
			
	    InFile.getline(str,81,'\n');
	    s=str;
	    head.iWeek              = atoi(s.Mid(3));
	    head.dSecond            = atof(s.Mid(8));
	    head.dInterval          = atof(s.Mid(24));
	    head.iMjdDay            = atoi(s.Mid(39));
	    head.dMjdDayFra         = atof(s.Mid(45));
		
		InFile.getline(str,81,'\n');
		s=str;
		head.iRealSatNumber     = atoi(s.Mid(4));
		if(head.iRealSatNumber>MAXNAVSAT)
			head.iRealSatNumber=29;   //   the others  are GLONASS satellites  in COD*.EPH FILE
 
    do 
	{	
		InFile.getline(str,81,'\n');
	}while(!(str[0]=='*')||!(str[1]==' '));


}

int READNAVSP3RECORD(ifstream InFile,ofstream OutFile,NAVSP3EPO *RecordEpo, int iEpoSatNum)
{
	InFile.seekg(-30,ios::cur);
	char str[82];
	CString s;
	int i=0;
    while(1)
	{	
		InFile.getline(str,82,'\n');
		if(strstr(str,"EOF"))
			break;
		if(strstr(str,"PR"))              //just for COD's EPh files  
			continue;
		if (str[0]==0)
				continue;
		s=str;
		RecordEpo[i].EpoTime.iYEAR   =  atoi(s.Mid(3));
		RecordEpo[i].EpoTime.iMONTH  =  atoi(s.Mid(8));
		RecordEpo[i].EpoTime.iDAY    =  atoi(s.Mid(11));
		RecordEpo[i].EpoTime.iHOUR   =  atoi(s.Mid(14));
		RecordEpo[i].EpoTime.iMINUTE =  atoi(s.Mid(17));
		RecordEpo[i].EpoTime.dSECOND =  atof(s.Mid(20));

		for(int j=0;j<iEpoSatNum;j++)
		{
			InFile.getline(str,82,'\n');
			if (str[0]==0)
				continue;
			if (str[0]=='*')
			{	printf("      The SP3 file is damaged for absence of satellite Pos record.\n");
				
				for(int isize=0;isize<82;isize++)
				if(str[isize]==0)
					break;
				
				InFile.seekg(-isize-4,ios::cur);
				break;
			}
			s=str;
			int iPRN    = atoi(s.Mid(2));
			RecordEpo[i].NavSp3[iPRN-1].iPrn = iPRN  ;
			RecordEpo[i].NavSp3[iPRN-1].xyzt.xyz.lX    = atof(s.Mid(4));
			RecordEpo[i].NavSp3[iPRN-1].xyzt.xyz.lY    = atof(s.Mid(18));
			RecordEpo[i].NavSp3[iPRN-1].xyzt.xyz.lZ    = atof(s.Mid(32));
			RecordEpo[i].NavSp3[iPRN-1].xyzt.lT        = atof(s.Mid(46));
		/*	if (strlen(str)>60)
			{
				RecordEpo[i].NavSp3[iPRN-1].lDX   = atof(s.Mid(60));
				RecordEpo[i].NavSp3[iPRN-1].lDY   = atof(s.Mid(63));
				RecordEpo[i].NavSp3[iPRN-1].lDZ   = atof(s.Mid(66));
				RecordEpo[i].NavSp3[iPRN-1].lDT   = atof(s.Mid(69));
			}*/
			
		}
		i++;
	}
	return 1;
}

int ININUMFROMTIME(double dEpoNum,int iPolyOrder)
{
	int    iTimeOrder=  int(dEpoNum/30);
	int    iIni=0;
	int    iSemiTimeOrder=(iPolyOrder-1)/2;
	if ((iTimeOrder>=iSemiTimeOrder)&&(iTimeOrder<(96-iSemiTimeOrder)))
		iIni=iTimeOrder-iSemiTimeOrder;
	if (iTimeOrder>=(96-iSemiTimeOrder))
		iIni=96-iPolyOrder;
	return iIni;
}

// 牛顿法内插卫星星历
void GETSATPOSFROMSP3(long double t,int iIni,int iPolyOrder,NAVSP3EPO *RecordEpo,long double **lCof,SATPOS &Pos)
{	int i;
	long double *xi;
	xi=new long double[iPolyOrder];
	GPSTIME time;
	UTCCOVGPS(&RecordEpo[iIni].EpoTime,&time);
	xi[0]=time.dWEEKSECONDS;
	for (i=1;i<iPolyOrder;i++)
	{		
		xi[i]=xi[i-1]+900;
	}
	Pos.time.dWEEKSECONDS=t;
	Pos.xyz.lX   =0,Pos.xyz.lY   =0,Pos.xyz.lZ   =0;
	Pos.Dotxyz.lX=0,Pos.Dotxyz.lY=0,Pos.Dotxyz.lZ=0;
    /**/
	long double lBase=1;
	for (i=0;i<iPolyOrder;i++)
	{	
		lBase*=(t-xi[i-1]);
		if(i==0)
			lBase=1.0;
		Pos.xyz.lX    +=(lCof[i][0]*lBase*1e+3);
		Pos.xyz.lY    +=(lCof[i][1]*lBase*1e+3);
		Pos.xyz.lZ    +=(lCof[i][2]*lBase*1e+3);
		//pSatClk[0]+=(lCof[i][3]*lBase*1e-6);
	}/**/
	long double lSum;
	for (i=0;i<iPolyOrder;i++)
	{	
		if(i==0)
		{
			lBase=1.0;lSum=0;
		}
		else
		{
			lBase*=(t-xi[i-1]);
			lSum+=(1/(t-xi[i-1]));
		}
		
		Pos.Dotxyz.lX    +=lSum*(lCof[i][0]*lBase*1e+3);
		Pos.Dotxyz.lY    +=lSum*(lCof[i][1]*lBase*1e+3);
		Pos.Dotxyz.lZ    +=lSum*(lCof[i][2]*lBase*1e+3);
	//	pSatClk[1]   +=lSum*(lCof[i][3]*lBase*1e-6);
	}/**/


}

void GETSATCLKFROMSP3(long double  t,int iIni,int iPolyOrder,NAVSP3EPO *RecordEpo,long double **lCof,long double *pSatClk)
{	int i;
	long double *xi;
	xi=new long double[iPolyOrder];
	GPSTIME time;
	UTCCOVGPS(&RecordEpo[iIni].EpoTime,&time);
	xi[0]=time.dWEEKSECONDS;
	for (i=1;i<iPolyOrder;i++)
	{		
		xi[i]=xi[i-1]+900;
	}
/*
	Pos.time.dWEEKSECONDS=t;
	Pos.xyz.lT   =0;
	Pos.Dotxyz.lX=0;*/

	pSatClk[0]=0;
	pSatClk[1]=0;
/**/
long double lBase=1;
for (i=0;i<iPolyOrder;i++)
{	
	lBase*=(t-xi[i-1]);
	if(i==0)
		lBase=1.0;
	/*
	Pos.xyz.lX    +=(lCof[i][0]*lBase*1e+3);
		Pos.xyz.lY    +=(lCof[i][1]*lBase*1e+3);
		Pos.xyz.lZ    +=(lCof[i][2]*lBase*1e+3);*/
	
	pSatClk[0]+=(lCof[i][0]*lBase*1e-6);
}/**/
long double lSum;
for (i=0;i<iPolyOrder;i++)
{	
	if(i==0)
	{
		lBase=1.0;lSum=0;
	}
	else
	{
		lBase*=(t-xi[i-1]);
		lSum+=(1/(t-xi[i-1]));
	}
	
/*
	Pos.Dotxyz.lX    +=lSum*(lCof[i][0]*lBase*1e+3);
	Pos.Dotxyz.lY    +=lSum*(lCof[i][1]*lBase*1e+3);
	Pos.Dotxyz.lZ    +=lSum*(lCof[i][2]*lBase*1e+3);*/
	pSatClk[1]   +=lSum*(lCof[i][0]*lBase*1e-6);
}/**/

	
}


// 拉格朗日法内插卫星星历

/*void GETSATPOSFROMSP3(double t ,int iTimeOrder ,int iSatPrn,NAVSP3EPO *RecordEpo,SATPOS &Pos,long double *pSatClk)
//iTimeOrder为UTC时转化为日秒在一天中15分间隔对应的顺序号
{	const int iPolyOrder=7;
	//实践检验，当iPolyOrder为13或以下值(如11)时其结果最为稳定
	//内插精度在几个厘米，中间历元可达2至3毫米，外推一个历元(15分钟)在几个分米
	const int iDim=4;
	      int iSemiTimeOrder=(iPolyOrder-1)/2;
	long double *dX=new long double[iPolyOrder];
	long double	**dY=TwoArrayAlloc(iPolyOrder,iDim);	

	long double dVec[2*iDim];
	VECTORLAGRANGE(t,dX,dY,iPolyOrder,dVec,iDim);
	TwoArrayFree(dY);
	delete dX;
	Pos.iPRN  =iSatPrn;
	Pos.dT    =t;
	Pos.xyz.lX    =dVec[0]*1e+3;
	Pos.xyz.lY    =dVec[1]*1e+3;
	Pos.dZ    =dVec[2]*1e+3;
	pSatClk[0]=dVec[3]*1e-6;
	Pos.Dotxyz.lX =dVec[4]*1e+3;
	Pos.Dotxyz.lY =dVec[5]*1e+3;
	Pos.Dotxyz.lZ =dVec[6]*1e+3;
	pSatClk[1]=dVec[7]*1e-6;
	  
}*/

⌨️ 快捷键说明

复制代码 Ctrl + C
搜索代码 Ctrl + F
全屏模式 F11
切换主题 Ctrl + Shift + D
显示快捷键 ?
增大字号 Ctrl + =
减小字号 Ctrl + -