getposotion.cpp

读取GPS的Rinex观测文件

#include "Getposition.h"
#include "refraction_delay.h"
#include "Matrix.h"


#ifndef _NO_NAMESPACE
using namespace std;
using namespace math;
#define STD std
#else
#define STD
#endif

#ifndef _NO_TEMPLATE
typedef matrix<double> Matrix;
#else
typedef matrix Matrix;
#endif

#ifndef _NO_EXCEPTION
#  define TRYBEGIN()	try {
#  define CATCHERROR()	} catch (const STD::exception& e) { \
						cerr << "Error: " << e.what() << endl; }
#else
#  define TRYBEGIN()
#  define CATCHERROR()
#endif

list<PGMNREC>::iterator GetBestGMNREC(list<PGMNREC>& navRecord,                                    //   找到最佳导航电文历元
				   int& nPRN,PCOMMONTIME pctEpoch) //卫星号及其对应时刻
{
	list<PGMNREC>::iterator it;
    bool flag=false;
	bool satPRN=false;
	int n=0;
	
	JULIANDAY sat_toe,sat_epoch;
    
	CommonTimeToJulianDay (pctEpoch,&sat_epoch);   

	for(it=navRecord.begin();it!=navRecord.end();it++)
	{
		if((*it)->PRN==nPRN)
		{
			satPRN=true;
			GPSTimeToJulianDay (&(*it)->TOE, &sat_toe);

			if(GetTimeDelta (&sat_epoch,&sat_toe)<0)
			{
				return it;
                flag=true;
				break;
			}
			else if(fabs(GetTimeDelta (&sat_epoch,&sat_toe))*86400<60*60)
			{
				return it;
				flag=true;
				break;
			}

//			n=i;
		}
		
	
	}
	if(!satPRN&&!flag)
	{
//		cout<<"the satPRN didn't exsit!"<<endl;
		return it=navRecord.begin();
      
	}
//	if(!flag)
  //       return sat_epoch;

}


double EofMe(double M,double e,double tol)
{
	double E0,E1;
	E0=M;
	E1=M+e*sin(E0);
	while(fabs(E1-E0)>tol)
	{
		E0=E1;
		E1=M+e*sin(E0);
     }
	return E1;

}

double Get_atan(double z,double y)
{
   double x;
   if (z==0) x=PI/2;
   else{
	if (y==0) x=PI;
	else{
	      x=atan(fabs(y/z));
	      if ((y>0)&&(z<0)) x=PI-x;
	      else if ((y<0)&&(z<0)) x=PI+x;
		   else if((y<0)&&(z>0)) x=2*PI-x;
	     }
       }
   return x;
}


void GetUtilParameter(list<PGMNREC>& navRecord,                                  //  记录卫星位置参数
				   int& nPRN,PCOMMONTIME pctEpoch,PUtilParam pParam)
{
	list<PGMNREC>::iterator  theBestGMN;
	theBestGMN=GetBestGMNREC(navRecord,nPRN,pctEpoch);

	//计算卫星平均角速度
	double n0=sqrt(GM)/ pow((*theBestGMN)->SqrtA,3);

	//计算相对于星历参考历元的时间

    JULIANDAY toe,epoch;
	CommonTimeToJulianDay (pctEpoch,&epoch);
	GPSTimeToJulianDay (&(*theBestGMN)->TOE, &toe);

	double tk;
	tk=GetTimeDelta (&epoch,&toe)*_DAY_IN_SECOND;
	if(tk>302400)
		tk-=604800;
	else if(tk<-302400)
		tk+=604800;
	else
		tk=tk;

	//对平均角速度进行改正
	double n=n0+(*theBestGMN)->DetlaN;

	(*pParam).n=n;

	//计算平近点角
	double M=(*theBestGMN)->M0+n*tk;

	//求偏近点角
    double E;
	E=EofMe(M,(*theBestGMN)->e,1e-10);

	(*pParam).E=E;

	//计算真近点角
	double vk,cosvk,sinvk;
	cosvk=(cos(E)-(*theBestGMN)->e)/(1-(*theBestGMN)->e*cos(E));
	sinvk=(sqrt(1-(*theBestGMN)->e*(*theBestGMN)->e)*sin(E))/(1-(*theBestGMN)->e*cos(E));

//	vk=atan2(sinvk,cosvk);
//	if(vk<0)
//	 vk+=2*PI;

	vk=Get_atan(cosvk,sinvk);
	 
	(*pParam).vk=vk;
	 //计算升交角距
	 double u0;
	 u0=(*theBestGMN)->omega+vk;
     (*pParam).u0=u0;

	 //计算二阶调和改正数
	     //1.计算升交角距的改正数
	 double detU=(*theBestGMN)->Cus*sin(2*u0)+(*theBestGMN)->Cus*cos(2*u0);
	     //2.计算向径的改正数
	 double detR=(*theBestGMN)->Crs*sin(2*u0)+(*theBestGMN)->Crs*cos(2*u0);
	     //3.计算轨道倾角改正数
	 double detI=(*theBestGMN)->Cis*sin(2*u0)+(*theBestGMN)->Cis*cos(2*u0);


	 //计算经过改正的升交角距
	 double uk=u0+detU;
	 (*pParam).uk=uk;

	 //计算经过改正的向径
	 double r=(*theBestGMN)->SqrtA*(*theBestGMN)->SqrtA*(1-(*theBestGMN)->e*cos(E))+detR;
	 (*pParam).r=r;

	 //计算经过改正的轨道倾角
	 double i=(*theBestGMN)->i0+detI+(*theBestGMN)->iDot*tk;
	 (*pParam).i=i;


	 //计算改正后的升交点经度
	 double L=(*theBestGMN)->Omega+((*theBestGMN)->OmegaDot-we)*tk
		 -we*((*theBestGMN)->TOE.tow.sn+(*theBestGMN)->TOE.tow.tos);
	 (*pParam).L=L;

}


void GetOrbNClk(list<PGMNREC>& navRecord,                                                            //计算卫星位置
				int& nPRN, PCOMMONTIME pctEpoch, PCRDCARTESIAN pcrdOrb)               //笛卡尔
{
    
    UtilParam pParam;
    GetUtilParameter(navRecord,nPRN,pctEpoch,&pParam);

	double n=pParam.n;
	double E=pParam.E;
	double vk=pParam.vk;
	double uk=pParam.uk;
	double r=pParam.r;
	double i=pParam.i;
	double L=pParam.L;
	double u0=pParam.u0;

	//计算卫星在轨道平面上的位置
	 double x1=r*cos(uk);
	 double y1=r*sin(uk);
	 double z1=0;

	 //计算在地固坐标系下的位置
	 pcrdOrb->x=cos(L)*x1-cos(i)*sin(L)*y1;
	 pcrdOrb->y=sin(L)*x1+cos(i)*cos(L)*y1;
	 pcrdOrb->z=sin(i)*y1;
//	 cout<<"x "<<pcrdOrb->x<<"   "<<"y "<<pcrdOrb->y<<"   "<<"z "<<pcrdOrb->z<<endl;	 
}



void GetSVClkBias(list<PGMNREC>& navRecord,int& nPRN,PCOMMONTIME pctEpoch,
				  double* pdSVClkBias,double *detj)
{
	UtilParam pParam;
    GetUtilParameter(navRecord,nPRN,pctEpoch,&pParam);

	list<PGMNREC>::iterator  theBestGMN;
	theBestGMN=GetBestGMNREC(navRecord,nPRN,pctEpoch);

	double E=pParam.E;
	
    JULIANDAY epoch;
	CommonTimeToJulianDay (pctEpoch,&epoch);
	//GPSTimeToJulianDay (&theBestGMN.TOE, &toe);

	 //计算C/A码信号发射时刻的改正
	 double dettr=F*(*theBestGMN)->e*(*theBestGMN)->SqrtA*sin(E);

	 JULIANDAY toc;
//	 GPSTimeToJulianDay (&(*theBestGMN)->TOC, &toc);
	 CommonTimeToJulianDay (&(*theBestGMN)->TOC, &toc);

	 double dettoc=GetTimeDelta (&epoch,&toc)*_DAY_IN_SECOND;

	 *pdSVClkBias=(*theBestGMN)->ClkBias+(*theBestGMN)->ClkDrift*dettoc
		 +(*theBestGMN)->ClkDriftRate*dettoc*dettoc+dettr;

	 *detj=(*theBestGMN)->ClkBias+(*theBestGMN)->ClkDrift*dettoc
		 +(*theBestGMN)->ClkDriftRate*dettoc*dettoc;
	 
}

void GetSatVelocity(list<PGMNREC>& navRecord,int& nPRN,PCOMMONTIME pctEpoch,                           //计算卫星的速度
					PSatVel psatv)
{
	UtilParam pParam;
    GetUtilParameter(navRecord,nPRN,pctEpoch,&pParam);

	list<PGMNREC>::iterator  theBestGMN;
    theBestGMN=GetBestGMNREC(navRecord,nPRN,pctEpoch);

	double n=pParam.n;
	double E=pParam.E;
	double vk=pParam.vk;
	double uk=pParam.uk;
	double r=pParam.r;
	double i=pParam.i;
	double L=pParam.L;
	double u0=pParam.u0;

	//计算卫星在轨道平面上的位置
	 double x1=r*cos(uk);
	 double y1=r*sin(uk);
	 double z1=0;

	 double Edot=n/(1-(*theBestGMN)->e*cos(E));
     double de=sqrt(1-(*theBestGMN)->e*(*theBestGMN)->e) * cos(E) * (cos(E)-(*theBestGMN)->e)+
	                sqrt(1-(*theBestGMN)->e*(*theBestGMN)->e)*sin(E)*sin(E);
     double dde=pow(cos(E)-(*theBestGMN)->e,2)+(1-(*theBestGMN)->e*(*theBestGMN)->e)*sin(E)*sin(E);
	 double U0dot=(de/dde)*Edot;
//U0dot实际是vk对 t的求导

	 double ukdot=(1+2*(*theBestGMN)->Cus*cos(2*u0)-2*(*theBestGMN)->Cuc*sin(2*u0))*U0dot;

	 double rkdot=(*theBestGMN)->SqrtA*(*theBestGMN)->SqrtA*(*theBestGMN)->e*sin(E)*Edot
		 +2*((*theBestGMN)->Crs*cos(2*u0)-(*theBestGMN)->Crc*sin(2*u0))*U0dot;

	 double Ikdot=(*theBestGMN)->iDot+2*((*theBestGMN)->Cis*cos(2*u0)-(*theBestGMN)->Cic*sin(2*u0))*U0dot;

	 double kdot=(*theBestGMN)->OmegaDot-we;//////


	 (*psatv).xv=cos(L)*(rkdot*cos(uk)-r*ukdot*sin(uk))
		      -sin(L)*cos(i)*(rkdot*sin(uk)+r*ukdot*cos(uk))
			  -(x1*sin(L)+y1*cos(L)*cos(i))*kdot
			  +y1*sin(L)*sin(i)*Ikdot;
	 (*psatv).yv=sin(L)*(rkdot*cos(uk)-r*ukdot*sin(uk))
		       +cos(L)*cos(i)*(rkdot*sin(uk)+r*ukdot*cos(uk))
			   +(x1*cos(L)-y1*sin(L)*cos(i))*kdot
			   -y1*cos(L)*sin(i)*Ikdot;
	 (*psatv).zv=sin(i)*(rkdot*sin(uk)+r*ukdot*cos(uk))
		        +y1*cos(i)*Ikdot;
}





void PPOne(GMOHDR& gmoh,/*观测值头文件*/
		   list<PGMOREC>::iterator pgmor,/*    *某一历元*    观测值 数据记录*/

		   list<PGMNREC>&  navRecord,/*导航电文数据文件*/
		   PPPONERESULT presult)
{
	
	list<PGMNREC>::iterator pgmnr;

	double toprelay;//对流层延迟
    int flag=0;//判断是否有D1，能否进行测速
	int ncount=0;//迭代次数
	////////////////////////////////////////////////////////////////////////////////     L1,L2,P1,P2,S1,S2,C1,D1,T1,L5
	int m_value=-1;//用到的观测文件的哪种伪距//C1,P1,P2，D1//                                L1,L2,   P2,      C1
	int m_L1=-1;   int m_L2=-1;
	int m_p1=-1;   int m_p2=-1;