gpsobsmsg.cpp

根据GPS卫星观测文件（.00O）与星历文件(.00N)

			pOneEpoch->ObsList.insert(iter_Obs,pObs);		
	    
    	
 }//end for1
 pObs = new GPSOBS;
 delete pObs;
 
  return *this;
}


GPSOBSMSG& GPSOBSMSG::GetPos(GPSNAVMSG &navMsg) //new
{
	OneEpochORec* pObsList;//一个历元的数据
	GPSTIME gTimeEpoch; 
	int numOfEpoch = this->pEpochObsList.size();
	int RealNumOfEpoch = numOfEpoch;
	int i,j;

	double X_average = 0;
	double Y_average = 0;
	double Z_average = 0;
	double XSigma_all = 0;
	double YSigma_all = 0;
	double ZSigma_all = 0;
	double ClkSigma_all = 0;

 
	ofstream resFile("res.txt");

  	this->pNavMsg = &navMsg;

	this->GetInitialPos();


	this->XYZ_to_BLH(this->X,this->Y,this->Z,this->B,this->L,this->H);

   for(i = 0; i < numOfEpoch;i++)
   {//for1
	pObsList = this->pEpochObsList[i];     

	int num = pObsList->Size();
	while(num < 4)  //当卫星数少于4时，不予计算
	{
		RealNumOfEpoch--;
		pObsList = this->pEpochObsList[i+1];
		num = pObsList->Size();		
	}

 	double X0 ;
 	double Y0 ;
 	double Z0 ;
	double dRecClkDtaTime0 = 0;
	dRecClkDtaTime = 0;  //接收机钟差
	double sigma0;

	Matrix<double>* pN = new Matrix<double>(4,4);  //法方程矩阵
	Matrix<double>* pDelta = new Matrix<double>(4,1);//用于存放结果
	
	this->BMatrix = new Matrix<double>(num,4);   //定义B,L矩阵
	this->LMatrix = new Matrix<double>(num,1);
	Matrix<double>* pV = new Matrix<double>(num,1);
	Matrix<double>* pSigma = new Matrix<double>(4,1);


	this->dRecClkDtaTime = 0 ;
	
	do 
	{
		

		CString strT;
//		strT.Format("%f",this->L); //used for test
//		AfxMessageBox(strT);


		X0=this->X ;
		Y0=this->Y ;
		Z0=this->Z ;
		dRecClkDtaTime0 = this->dRecClkDtaTime;

		pObsList->tmEpoch.ConvertToGpsTime(gTimeEpoch);//一个历元的时间
		
		for(j = 1;j<= num;j++)
		{//获取矩阵的参数值
			this->GetValueOfMatrixElement(gTimeEpoch,*(pObsList->ObsList[j-1]),dRecClkDtaTime0,
				(*BMatrix)(j,1),(*BMatrix)(j,2),(*BMatrix)(j,3),(*BMatrix)(j,4),(*LMatrix)(j,1));						
		}
		//计算法方程
		(*pN) = ((this->BMatrix->TransPose()) * (*this->BMatrix)).Inverse();
        //计算改正数
		(*pDelta) = (*pN) * (this->BMatrix->TransPose()) *  (*this->LMatrix);

		this->X = X0 + (*pDelta)(1,1);
		this->Y = Y0 + (*pDelta)(2,1);
		this->Z = Z0 + (*pDelta)(3,1);
		this->dRecClkDtaTime = dRecClkDtaTime0 - (*pDelta)(4,1)/VC;
 
		
	}while(Distance(X,Y,Z,X0,Y0,Z0) > 0.001);

	(*pV) = (*this->BMatrix) * (*pDelta) - (*this->LMatrix);
	sigma0 = (pV->TransPose() * (*pV))(1,1)/(num - 4);
	(*pSigma)(1,1) = sigma0*(*pN)(1,1);
	(*pSigma)(2,1) = sigma0*(*pN)(2,2);
	(*pSigma)(3,1) = sigma0*(*pN)(3,3);
	(*pSigma)(4,1) = sigma0*(*pN)(4,4);

    resFile.precision(12);
 //	resFile<<this->X<<"\t"<<this->Y<<"\t"<<this->Z<<"\t"<<
 //	    	this->dRecClkDtaTime<<endl;
	resFile<<"\t"<<sqrt((*pSigma)(1,1))<<"\t"<<sqrt((*pSigma)(2,1))
			<<"\t"<<sqrt((*pSigma)(3,1))<<"\t"<<sqrt((*pSigma)(4,1))<<endl;
    X_average += this->X;
	Y_average += this->Y;
	Z_average += this->Z;
	XSigma_all += (*pSigma)(1,1);
	YSigma_all += (*pSigma)(2,1);
	ZSigma_all += (*pSigma)(3,1);

   
	delete pN;
	delete pDelta;
	delete pV;
	delete pSigma;

	delete this->BMatrix;
	delete this->LMatrix;
   }//for1

    X_average /= RealNumOfEpoch;
	Y_average /= RealNumOfEpoch;
	Z_average /= RealNumOfEpoch;
	XSigma_all = sqrt(XSigma_all/RealNumOfEpoch);
	YSigma_all = sqrt(YSigma_all/RealNumOfEpoch);
	ZSigma_all = sqrt(ZSigma_all/RealNumOfEpoch);
	resFile<<"//////////////////////////////////\n"
		<<X_average<<"\t"<<Y_average<<"\t"<<Z_average<<"\t"
		<<XSigma_all<<"\t"<<YSigma_all<<"\t"<<ZSigma_all<<endl;

	return *this;

}

void GPSOBSMSG::GetValueOfMatrixElement(GPSTIME& gTimeEpoch,GPSOBS& gObs,double dRecClkDtaTime,
							double &b1, double &b2, double &b3, double &b4, double& l)//new
{
	double dTranTime;//信号传播时间
	double newTranTime;//用于存放信号传播时间
	double dSVClkTime;//卫星钟偏差
	double dSVClkRelation;//相对论效应引起的卫星钟差
	GPSTIME shootTime;//信号发射时刻
	double XSV;
	double YSV;
	double ZSV;
	double PR_Survey = gObs.dC1;  //伪矩观测值
	double PR;//站星距离（计算出来的）
	oneGPSNAVMSGREC nearTimeNavRec;


	this->pNavMsg->GetNearTimeNavRec(gObs.byPRN,gTimeEpoch,nearTimeNavRec);
    //计算卫星钟差
	dSVClkTime = nearTimeNavRec.GetSVClkBias(gTimeEpoch);      
	
	dTranTime = PR_Survey / VC - dRecClkDtaTime + dSVClkTime ;
    newTranTime = dTranTime;
	do {
				
	    cout.precision(12);
		dTranTime = newTranTime;	
	    shootTime = gTimeEpoch - dTranTime;
    
    	nearTimeNavRec.GetSVPos(shootTime,XSV,YSV,ZSV);    //卫星位置可能有错
 	    this->Earthrot(dTranTime,XSV,YSV,ZSV);
	
 
		newTranTime = Distance(this->X,this->Y,this->Z,XSV,YSV,ZSV) / VC;
	}while(fabs(newTranTime - dTranTime)>1.0e-12);

     dSVClkRelation = nearTimeNavRec.GetSVClkRelation();  


	 

	 //计算对流层延迟
	 double ele;
	 double dr = 0.0;//对流层延迟量（米）

	 double IonDelay = 0.0;


	 CString strT;
	 if (this->B != 0)
	 {
		 ele = this->GetElevation(this->X,this->Y,this->Z,XSV,YSV,ZSV) * PI/180;
		 this->GetTrop(PI/2.0 - ele,this->H,dr);
		 
//	   strT.Format("%f",ele ); //used for test
//  	AfxMessageBox(strT);

	 //计算电离层延迟
	 
	 IonDelay = nearTimeNavRec.GetIonDelay(ele);
	 }






	PR = Distance(this->X,this->Y,this->Z,XSV,YSV,ZSV); 
	b1 =  (this->X - XSV)/PR;
	b2 =  (this->Y - YSV)/PR;
	b3 =  (this->Z - ZSV)/PR;
	b4 =  -1.0;

     


	l = PR_Survey - PR + VC * (dSVClkTime - dRecClkDtaTime  - dSVClkRelation) - dr + IonDelay;
  
}

void GPSOBSMSG::Earthrot(const double dTranTime, double &XSV, double &YSV, double &ZSV)//new
{
	int i, j;
    double R[3][3], Xx[3], v[3], cl, sl, omiga, alpha;
    omiga= PI*2.0/(24.0*3600.0);
    alpha= omiga*dTranTime;
	
 
    cl= cos(alpha);
	sl= sin(alpha);
	R[0][0] = cl; R[0][1] = sl; R[0][2] = 0.0; /*rotation matrix R*/
	R[1][0] = -sl; R[1][1] = cl; R[1][2] = 0.0;
	R[2][0] = 0.0; R[2][1] = 0.0; R[2][2] = 1.0;
	Xx[0]=XSV ; Xx[1]=YSV ; Xx[2]=ZSV;
    /* vector rotate */
	for(i=0;i<3;i++)
	{
		v[i] = 0.0;
		for(j=0;j<3;j++)
			v[i] += R[i][j]*Xx[j];
	}
	//return satellite position after plusingthe earth rotation correction
    XSV = v[0];
	YSV = v[1];
	ZSV = v[2];

}

GPSOBSMSG& GPSOBSMSG::GetInitialPos()
{
	Matrix<double>* pBMatrix;
	pBMatrix = new Matrix<double>(4,4);
	Matrix<double>* pLMatrix;
	pLMatrix = new Matrix<double>(4,1);

	Matrix<double>* result = new Matrix<double>(4,1);

	int i ;
	GPSTIME gTimeEpoch;
	OneEpochORec* pObsList = this->pEpochObsList[0];
	pObsList->tmEpoch.ConvertToGpsTime(gTimeEpoch);


	for(i = 0;i < 4;i++)
	{
		this->GetValueOfMatrixElement(gTimeEpoch,*(pObsList->ObsList[i]),0.0,(*pBMatrix)(i+1,1),
			(*pBMatrix)(i+1,2),(*pBMatrix)(i+1,3),(*pBMatrix)(i+1,4),(*pLMatrix)(i+1,1));	
	}
 	*result = pBMatrix->Inverse() * (*pLMatrix);  
  
	this->X = (*result)(1,1);
	this->Y = (*result)(2,1);
	this->Z = (*result)(3,1);


   delete pBMatrix;
   delete pLMatrix;





    double X0,Y0,Z0;
	double dRecClkDtaTime0;
	this->dRecClkDtaTime = 0;  //接收机钟差

	int num = pObsList->Size();
	int j;

	Matrix<double>* pN = new Matrix<double>(4,4);  //法方程矩阵
	Matrix<double>* pDelta = new Matrix<double>(4,1);//用于存放结果
	
    pBMatrix = new Matrix<double>(num,4);   //定义B,L矩阵
	pLMatrix = new Matrix<double>(num,1);
	do 
	{
		
		X0=this->X ;
		Y0=this->Y ;
		Z0=this->Z ;
		dRecClkDtaTime0 = this->dRecClkDtaTime;

		pObsList->tmEpoch.ConvertToGpsTime(gTimeEpoch);//一个历元的时间
		
		for(j = 1;j<= num;j++)
		{//获取矩阵的参数值
			this->GetValueOfMatrixElement(gTimeEpoch,*(pObsList->ObsList[j-1]),dRecClkDtaTime0,
				(*pBMatrix)(j,1), (*pBMatrix)(j,2) ,(*pBMatrix)(j,3),(*pBMatrix)(j,4),(*pLMatrix)(j,1));						
 
		}
		//计算法方程
		(*pN) = ((pBMatrix->TransPose()) * (*pBMatrix)).Inverse();
        //计算改正数
		(*pDelta) = (*pN) * (pBMatrix->TransPose()) *  (*pLMatrix);

		this->X = X0 + (*pDelta)(1,1);
		this->Y = Y0 + (*pDelta)(2,1);
		this->Z = Z0 + (*pDelta)(3,1);
		this->dRecClkDtaTime = dRecClkDtaTime0 - (*pDelta)(4,1)/VC;
 
		
	}while(Distance(X,Y,Z,X0,Y0,Z0) > 0.001);

 //   CString str;
//	str.Format("%f,%f,%f",this->X,this->Y,this->Z);
//	AfxMessageBox(str);
	delete pN;
	delete pDelta;
	delete pBMatrix;
	delete pLMatrix;

	
	return *this;

}
GPSOBSMSG::~GPSOBSMSG()
{
	for_each(this->pEpochObsList.begin(),this->pEpochObsList.end(),CDeleteObject());
	this->pEpochObsList.clear();

}

void  GPSOBSMSG::XYZ_to_BLH(const double& x1,const double& y1,const double&z1, double& B, double& L, double& H)
/* 84坐标 X,Y,Z-->B,L,h  */
{
	////见大地测量学
	double N;
	double a=6378137;
	double e2=0.0066943799013;
	double e12=0.00673949674227;
    double B_rad0=0;
    double B_rad=0;
	B_rad0=atan(Z*(1+e12)/(sqrt(X*X+Y*Y)));
	do{
		 B_rad=B_rad0;
         N=a/sqrt(1-e2*sin(B_rad)*sin(B_rad));
         B_rad0=atan((Z+N*e2*sin(B_rad))/(sqrt(X*X+Y*Y)));
    }while(fabs(B_rad-B_rad0)>0.0000000001);
	double    B_deg=B_rad*180/PI;
	B=B_deg;

	double L_rad=atan(Y/X);
	double L_deg=180+L_rad*180/PI;
    L_rad=L_rad+PI;
    L=L_deg;
	double h=sqrt(X*X+Y*Y)/cos(B_rad)-N;
	H=h;
}


void GPSOBSMSG::XYZ_to_NEU(double deltaX, double deltaY, double deltaZ, double B, double L, double &deltaN, double &deltaE, double &deltaH)
{//参心坐标到站心坐标的转换函数
 //可得出卫星位置在站心坐标系的坐标
	
   deltaN=-sin(B)*cos(L)*deltaX-sin(B)*sin(L)*deltaY+cos(B)*deltaZ;
   deltaE=-sin(L)*deltaX+cos(L)*deltaY;
   deltaH=cos(B)*cos(L)*deltaX+cos(B)*sin(L)*deltaY+sin(B)*deltaZ;

}

double GPSOBSMSG::GetElevation(const double &x1, const double &y1, const double &z1, const double &x2, const double &y2, const double &z2)
{//获取卫星的高度角
	double elevation;
	double deltaN,deltaE,deltaH;

   if(this->B ==0) //即还没有计算BLH
	   XYZ_to_BLH(x1,y1,z1,this->B,this->L,this->H);
    double BB=B*PI/180.00;
	double LL=L*PI/180.00;
	
	XYZ_to_NEU(x2-x1,y2-y1,z2-z1,BB, LL,deltaN,deltaE,deltaH);
	elevation=atan(deltaH/sqrt(deltaN*deltaN+deltaE*deltaE));
    elevation=elevation*180/PI;

	return elevation;

}

void GPSOBSMSG::GetTrop(double Z, double HS, double &DR)
{
	//Z 天顶距
    //double HS 大地高 H  单位km
    //使用方法：伪距减对流层改正
	double T,P,RH;
	double T0=18.0;//standard metero parameter
	double P0=1013.25;
	double RH0=50.0;
	T =T0-0.0065*HS;
	P =P0*pow((1-2.26E-5*HS),5.225);
	RH=RH0*exp(-6.396E-4*HS);

	double ALPHA[10],H[2],N[2];
	double Z4,H4,T4,P4,RH4;
	double BCOR[6]={1.156,1.006,0.874,0.757,0.654,0.563};
	double HTAB[8]={0.0,0.5,1.0,1.5,2.0,3.0,4.0,5.0};
	double ZTAB[13]={60.0,66.0,70.0,73.0,75.0,76.0,77.0,78.0,78.5,79.0,79.5,
		79.75,80.0};

	Z4=Z;
	H4=HS;
	T4=T+273.15;
	P4=P;
	if(RH>100.0)
		RH4=100.0;
	else
		RH4=RH;

	// WATER VAPOR PRESSURE
	double E=RH4/100.0*exp(-37.2465+0.213166*T4-0.000256908*T4*T4);
	// NO MODEL AT ALL
	if (this->Model==0) 
	{
		DR=0.0;
		return;
	}
	if(this->Model==1)
	{
		// MODEL 1: SAASTAMOINEN
		// HEIGHT IN KM
		double HL=H4/1000.0;
		if(HL<=0.0) HL=0.0;
		if(HL>=5.0) HL=5.0;
		int I=(int)HL;
		//REFERENCE HEIGHT FOR LINEAR INTERPOLATION IN TABLE BCOR
		double HREF=I;
		double BB=BCOR[I]+(BCOR[I+1]-BCOR[I])*(HL-HREF) + 0.716*1.0E-8*HS;
		//ROPOSPHERIC DISTANCE CORRECTION DR
		DR=2.277E-3*(P4+(1225/T4+0.05)*E-BB*pow(tan(Z4),2))/cos(Z4);
		return;
	}
	
	if(this->Model==2)
	{
		// MODEL 2: MODIFIED HOPFIELD
		N[0]=0.776E-4*P4/T4;
		N[1]=0.373*E/pow(T4,2.0);
		//H[0]=40.136+0.14872*(T4-273.16);
		//H[1]=11.0;
		H[0] = 40316 + 148.72 * (T4 - 273.16);
		H[1] = 11000;
		
		// ELEVATION ANGLE
		double THETA=PI/2.0-Z4;
		//double AE=6378.0+H4/1000.0;

		double AE = 6378000 + H4;

		DR=0.0;
		double SINTH=sin(THETA);
		double COSTH=cos(THETA);
		for(int i=0;i<2;i++)
		{
			double R=sqrt(pow((AE+H[i]),2)-pow((AE*COSTH),2))-AE*SINTH;
			double A=-SINTH/H[i];
			double B=-pow(COSTH,2)/(2.0*H[i]*AE);
			ALPHA[1]=1.0;
			ALPHA[2]=4.0*A;
			ALPHA[3]=6.0*A*A+4.0*B;
			ALPHA[4]=4.0*A*(A*A+3.0*B);
			ALPHA[5]=pow(A,4)+12.0*A*A*B+6.0*B*B;
			ALPHA[6]=4.0*A*B*(A*A+3.0*B);
			ALPHA[7]=B*B*(6.0*A*A+4.0*B);
			ALPHA[8]=4.0*A*pow(B,3);
			ALPHA[9]=pow(B,4);
			double DRI=0.0;			
			for(int j=1;j<10;j++)
				DRI=DRI+ALPHA[j]/j*pow(R,j);		
			DR=DR+1.0E-6*DRI*N[i];
		}
		return;
	}
	if(this->Model==3)
	{
		// MODEL 3: SIMPLIFIED HOPFIELD
		double ELEV=90.0-Z4*180.0/PI;
		double KD=1.552E-5*P4/T4*((148.72*T4-488.3552)-H4);
		double KW=7.46512E-2*E/pow(T4,2)*(11000.0-H4);
		DR=KD/sin(sqrt((pow(ELEV,2)+6.25))*PI/180.0)+KW/sin(sqrt((pow(ELEV,2)+2.25))*PI/180.0);
		return;
	}

}

double GPSOBSMSG::GetAzimuthAngle(const double &x1, const double &y1, const double &z1, const double &x2, const double &y2, const double &z2)
{
	//获取观测卫星的方向角(rad)
	double Azimuth;
	double deltaN,deltaE,deltaH;

   if(this->B ==0) //即还没有计算BLH
	   XYZ_to_BLH(x1,y1,z1,this->B,this->L,this->H);
    double BB=B*PI/180.00;
	double LL=L*PI/180.00;
	
	XYZ_to_NEU(x2-x1,y2-y1,z2-z1,BB, LL,deltaN,deltaE,deltaH);
	Azimuth = atan2(deltaE,deltaN);
    Azimuth = Azimuth * 180/PI;

	return Azimuth;

}