phase.cpp

GPS 定位授时源码

#include "stdafx.h"
#include "Phase.h"
#include "iomanip.h"
#include "math.h"
#include "SP3.h"
#include "Matrixcsl.h"


void CycleSlipDetectionMW(ofstream pOut,SATOBSREC *SatObsRec,int *iEpoNumofSat,int *obstype)
{   //see Geoffery Blewitt An automatic editing algorithm for GPS data
	//const double dWaveLength1=VC/FREQ_L1;
	//const double dWaveLength2=VC/FREQ_L2;   //have defined 
    const long double lWavelengthDelta=VC/(FREQ_L1-FREQ_L2);
	
	int       i,j;
	int       iNumInArc=0;              //The current number in the arc
	for (i=0;i<31;i++)
	{	
		for(j=0;j<iEpoNumofSat[i];j++)
		if(SatObsRec[i].PObsEpoRec[j])
		{	OBSRECPERSAT *Tem = SatObsRec[i].PObsEpoRec[j];			
			Tem->lLDelta=(Tem->lOBS[obstype[L1]]-Tem->lOBS[obstype[L2]])*VC/(FREQ_L1-FREQ_L2);
			Tem->lPDelta=(Tem->lOBS[obstype[P1]]*FREQ_L1+Tem->lOBS[obstype[P2]]*FREQ_L2)/(FREQ_L1+FREQ_L2);
			Tem->lBDelta=(Tem->lLDelta-Tem->lPDelta)/lWavelengthDelta;	
			if (Tem->iCycleFlag>=1)
			    Tem->lDeltaB=0.5;  //   A new satellite occurs   Marks 1
			
			else Tem->lDeltaB=0;
		
		}
	}
	/////////////////////////////////////////////////////////
	for (i=0;i<31;i++)
	{
		for(j=0;j<iEpoNumofSat[i];j++,iNumInArc++)
		if(SatObsRec[i].PObsEpoRec[j])
		{
			OBSRECPERSAT * TemOld = SatObsRec[i].PObsEpoRec[j-1];
			/**************************************************************地址错误*/
			OBSRECPERSAT * TemNew = SatObsRec[i].PObsEpoRec[j];
			OBSRECPERSAT * TemNext= SatObsRec[i].PObsEpoRec[j+1];
			long double lTem    =TemNew->lBDelta,//for store the primate Temnew.lBDelta
						lTemNew ,				 //for store the computed Temnew.lBDelta
						lTemDelta;			     //for store the computed Temnew.lDeltaB

			TemNew->iCycleFlag
            //deal with the first epoch of the satellite
			if (TemNew->iCycleFlag>0)
			{
				iNumInArc=1;
// 				pOut<<SatObsRec[i].PObsEpoRec[j]->iPRN<<"  "<<setw(4)<<j<<" "<<setw(16)<<setprecision(10)
// 					<<SatObsRec[i].PObsEpoRec[j]->lBDelta<<" "<<setw(14)<<setprecision(10)<<SatObsRec[i].PObsEpoRec[j]->lDeltaB<<"  "<<SatObsRec[i].PObsEpoRec[j]->iCycleFlag<<endl;
 				continue;
			}
			if(TemNew->iCycleFlag<0)
				continue;
			lTemNew  =TemOld->lBDelta +(lTem-TemOld->lBDelta)/iNumInArc;//i+1 refers to point number in the connected arc
			lTemDelta=pow(TemOld->lDeltaB,2)+(pow((lTem-TemOld->lBDelta),2)-pow(TemOld->lDeltaB,2))/iNumInArc;
			lTemDelta=pow(lTemDelta,0.5);
	
			if (fabs(TemNew->lBDelta-TemOld->lBDelta)<(4*TemOld->lDeltaB))
			{	
				TemNew->lBDelta=lTemNew;
				TemNew->lDeltaB=lTemDelta;
			}
			else if (fabs(TemNew->lBDelta-TemOld->lBDelta)>=(4*TemOld->lDeltaB))
			{
				if(TemNext&&(fabs(TemNext->lBDelta-TemOld->lBDelta)>=(4*TemOld->lDeltaB)))
				{	//TemNext&&  is very important for use the next epoch
					SatObsRec[i].PObsEpoRec[j]->iCycleFlag=2;         // cycle slips occur
					SatObsRec[i].PObsEpoRec[j]->lDeltaB=0.5;
					iNumInArc=1;
				}
				else                                       //isolated outliers deleted 
				{
					SatObsRec[i].PObsEpoRec[j]->lOBS[obstype[L1]]=0;
					SatObsRec[i].PObsEpoRec[j]->lOBS[obstype[L2]]=0;
					SatObsRec[i].PObsEpoRec[j]->lBDelta=SatObsRec[i].PObsEpoRec[j-1]->lBDelta;
					SatObsRec[i].PObsEpoRec[j]->lDeltaB=SatObsRec[i].PObsEpoRec[j-1]->lDeltaB;
					SatObsRec[i].PObsEpoRec[j]->iCycleFlag=-1;
					// -1 means observation of the epoch is bad,but the next one is also useful
					
				}
			}
			/*pOut<<SatObsRec[i].PObsEpoRec[j]->iPRN<<"  "<<setw(4)<<j<<" "
				<<setw(16)<<setprecision(10)<<SatObsRec[i].PObsEpoRec[j]->lBDelta
				<<" "<<setw(14)<<setprecision(10)<<SatObsRec[i].PObsEpoRec[j]->lDeltaB
				<<"  "<<SatObsRec[i].PObsEpoRec[j]->iCycleFlag<<endl;*/
		}
	}
}

//at the present time，This function is based on CycleSlipDetectionMW cycle slip deal with 
//cycle slip beforehand and then detect the cycle slip when DeltaN1＝DeltaN2
//The old type ---the runtime of the function is too long 
/*void CycleSlipDetectionIon(ofstream pOut,SATOBSREC *SatObsRec,int *iEpoNumofSat,int *obstype)
{
	int i,j=0,k;
	BOOL bBreak=FALSE;
	//int               iArcTotalNumIn=0;              //The Total number in the arc
	long double lWaveLengthIono =WAVELENGTH2 -WAVELENGTH1;   //be not very strict in meanings 
	for (i=0;i<31;i++)
	{
		for (j=0;j<iEpoNumofSat[i];j++)
		if(SatObsRec[i].PObsEpoRec[j])
		{	
			OBSRECPERSAT *Tem=SatObsRec[i].PObsEpoRec[j];
			Tem->lLIono=Tem->lOBS[obstype[L1]]*WAVELENGTH1-Tem->lOBS[obstype[L2]]*WAVELENGTH2; 
		    if (!j||(SatObsRec[i].PObsEpoRec[j-1]==NULL))
				Tem->iCycleFlag=1;//   A new satellite occurs   Marks 1
		}
	}
	for (i=0;i<31;i++)
	{
		for (j=0;j<iEpoNumofSat[i]-8;j++)
		{  		
			long double *lQlast=new long double[2],
						*lQnew =new long double[2],
						*lQnext=new long double[2];
			long double *lX=new long double[3],
						**lY=TwoArrayAlloc(3,1);
			bBreak=FALSE;	
// 			lX[0]=j;
//			lY[0][0]=SatObsRec[i].PObsEpoRec[j]->lLIono;
			for(k=j;k<(j+6);k++)
			if(!SatObsRec[i].PObsEpoRec[k]){	
				{bBreak=true;
				break;}
			}
		
			if(bBreak)
			{	j+=1;
				continue;
			}

			int p=0;
			for(k=j;p<3;k++)
			{   if((!SatObsRec[i].PObsEpoRec[k])||(SatObsRec[i].PObsEpoRec[k]->iCycleFlag==2))
				{bBreak=TRUE;
				break;}
				if((SatObsRec[i].PObsEpoRec[k]->iCycleFlag==(-1)))
					continue;
			
				lX[p]=k;
				lY[p][0]=SatObsRec[i].PObsEpoRec[k]->lLIono;
				p++;
	
			}
			if(bBreak)
			{	j+=4;
				continue;
				}
			
			VECTORLAGRANGE(j+3,lX,lY,2,lQlast,1);
			VECTORLAGRANGE(j+4,lX,lY,2,lQnew,1);
			VECTORLAGRANGE(j+5,lX,lY,2,lQnext,1);
			long double lDLast=SatObsRec[i].PObsEpoRec[j+3]->lLIono-lQlast[0],
						lDNew =SatObsRec[i].PObsEpoRec[j+4]->lLIono-lQnew[0],
						lDNext=SatObsRec[i].PObsEpoRec[j+5]->lLIono-lQnext[0];

			if((abs(lDNew-lDLast)>(6*lWaveLengthIono))&&(abs(lDNext-lDNew)<(6*lWaveLengthIono)))
				SatObsRec[i].PObsEpoRec[j+4]->iCycleFlag=2;	
			//cycle slip occur
			if((abs(lDNew-lDLast)>(6*lWaveLengthIono))&&(abs(lDNext-lDLast)<(6*lWaveLengthIono)))
				SatObsRec[i].PObsEpoRec[j+4]->iCycleFlag=-1;
			//outliers occur
			if(abs(lDNext-lDNew)>(6*lWaveLengthIono))
				SatObsRec[i].PObsEpoRec[j+5]->iCycleFlag=2;
			// a cursory judgement
			

			delete[]lQlast;
			delete[]lQnew;
			delete[]lQnext;
			delete[]lX;
			TwoArrayFree(lY);
		}
 		for (j=0;j<iEpoNumofSat[i];j++)
		{	if(SatObsRec[i].PObsEpoRec[j])	
 			{long double lPI=SatObsRec[i].PObsEpoRec[j]->lOBS[obstype[P2]]-SatObsRec[i].PObsEpoRec[j]->lOBS[obstype[P1]];
			pOut<<SatObsRec[i].PObsEpoRec[j]->iPRN<<"  "<<setw(4)<<j<<" "<<setw(14)<<setprecision(7)<<SatObsRec[i].PObsEpoRec[j]->lLIono<<"  "<<SatObsRec[i].PObsEpoRec[j]->iCycleFlag<<endl;
			}
		}
		
	}
}
*/
//at the present time，This function is based on CycleSlipDetectionMW cycle slip deal with 
//cycle slip beforehand and then detect the cycle slip when DeltaN1＝DeltaN2
//The new type.
void CycleSlipDetectionIon(ofstream pOut,SATOBSREC *SatObsRec,int *iEpoNumofSat,int *obstype)
{	int i,j;
	int iArcNum;
	long double lWaveLengthIono =WAVELENGTH2 -WAVELENGTH1;   //be not very strict in meanings 
	for (i=0;i<31;i++)
	{
		for (j=0;j<iEpoNumofSat[i];j++,iArcNum++)
		if(SatObsRec[i].PObsEpoRec[j])
		{	if(!SatObsRec[i].PObsEpoRec[j]->lOBS[obstype[L1]]||!SatObsRec[i].PObsEpoRec[j]->lOBS[obstype[L2]])
			{	SatObsRec[i].PObsEpoRec[j]->iCycleFlag=-1;
				if(SatObsRec[i].PObsEpoRec[j+1])
					SatObsRec[i].PObsEpoRec[j+1]->iCycleFlag=1;
				cout<<"   At the eoch there are no L phase observations."<<endl;
				continue;}
			if(!SatObsRec[i].PObsEpoRec[j]->lOBS[obstype[P1]]||!SatObsRec[i].PObsEpoRec[j]->lOBS[obstype[P2]])
			{	SatObsRec[i].PObsEpoRec[j]->iCycleFlag=-1;
				cout<<"   At the eoch there are no P Code observations."<<endl;
			}
			OBSRECPERSAT *Tem= SatObsRec[i].PObsEpoRec[j];
			SatObsRec[i].PObsEpoRec[j]->lLIono=Tem->lOBS[obstype[L1]]*WAVELENGTH1-Tem->lOBS[obstype[L2]]*WAVELENGTH2; 		
		}
	}
	for (i=0;i<31;i++)
	{
		for (j=0;j<iEpoNumofSat[i];j++,iArcNum++)
		if(SatObsRec[i].PObsEpoRec[j])
		{	
		    OBSRECPERSAT *TemNew= SatObsRec[i].PObsEpoRec[j];
			if (TemNew->iCycleFlag>0)
			{
				iArcNum=1;//use lDeltaB store the variance of the lLIono ,it can be see as lDeltaIon
				//SatObsRec[i].PObsEpoRec[j]->lLIono=Tem->lLIono;
				SatObsRec[i].PObsEpoRec[j]->lDeltaB =0.5;
// 				pOut<<SatObsRec[i].PObsEpoRec[j]->iPRN<<"  "<<setw(4)<<j<<" "
// 				<<setw(16)<<setprecision(10)<<SatObsRec[i].PObsEpoRec[j]->lLIono
// 				<<" "<<setw(14)<<setprecision(10)<<SatObsRec[i].PObsEpoRec[j]->lDeltaB
// 				<<"  "<<SatObsRec[i].PObsEpoRec[j]->iCycleFlag<<endl;
				continue;
			}
			if(TemNew->iCycleFlag<0)
				continue;
			OBSRECPERSAT * TemOld = SatObsRec[i].PObsEpoRec[j-1];
			OBSRECPERSAT * TemNext= SatObsRec[i].PObsEpoRec[j+1];

			long double  lTem; // temporarily store the Tem->lLIon
		    lTem=TemOld->lLIono+(TemNew->lLIono-TemOld->lLIono)/iArcNum;//i+1 refers to point number in the connected arc
			long double lTemDeltaIon=pow(TemOld->lDeltaB,2)+(pow((TemNew->lLIono-TemOld->lLIono),2)-pow(TemOld->lDeltaB,2))/iArcNum;
			lTemDeltaIon=pow(lTemDeltaIon,0.5);
	
			if (fabs(TemNew->lLIono-TemOld->lLIono)<(4.0*TemOld->lDeltaB))
			{	
				SatObsRec[i].PObsEpoRec[j]->lLIono=lTem;
				SatObsRec[i].PObsEpoRec[j]->lDeltaB=lTemDeltaIon;
			}
			else if (fabs(TemNew->lLIono-TemOld->lLIono)>=(4.0*TemOld->lDeltaB))
			{
				if(TemNext&&(fabs(TemNext->lLIono-TemOld->lLIono)>=(4.0*TemOld->lDeltaB)))
				{	//TemNext&&  is very important for use the next epoch
					SatObsRec[i].PObsEpoRec[j]->iCycleFlag=2;         // cycle slips occur
					SatObsRec[i].PObsEpoRec[j]->lDeltaB=0.5;
					iArcNum=1;
				}
				else                                       //isolated outliers deleted 
				{
					SatObsRec[i].PObsEpoRec[j]->lOBS[obstype[L1]]=0;
					SatObsRec[i].PObsEpoRec[j]->lOBS[obstype[L2]]=0;
					SatObsRec[i].PObsEpoRec[j]->lLIono=SatObsRec[i].PObsEpoRec[j-1]->lLIono;
					SatObsRec[i].PObsEpoRec[j]->lDeltaB=SatObsRec[i].PObsEpoRec[j-1]->lDeltaB;
					SatObsRec[i].PObsEpoRec[j]->iCycleFlag=-1;
					// -1 means observation of the epoch is bad,but the next one is also useful
					
				}
			}
			/*pOut<<SatObsRec[i].PObsEpoRec[j]->iPRN<<"  "<<setw(4)<<j<<" "
				<<setw(16)<<setprecision(10)<<SatObsRec[i].PObsEpoRec[j]->lLIono
				<<" "<<setw(14)<<setprecision(10)<<SatObsRec[i].PObsEpoRec[j]->lDeltaB
				<<"  "<<SatObsRec[i].PObsEpoRec[j]->iCycleFlag<<endl;*/

		}
	}

}

int  SatObsRecIni(SATOBSREC *SatObsRec,OBSREC *obsrec,int * p,int iTotalNum)
{//added in 2005.5.22 
 //The purpose is creat a data struct of different satellite arranged in sequence of epoch
	int i,j,k;
	
	for (i=0;i<31;i++)
	for (j=0;j<iTotalNum;j++)
	{
		SatObsRec[i].PObsEpoRec[j]=NULL;
	}
	for( i=0;i<MAXNAVSAT;i++)
		p[i]=0;
	for (j=0;j<iTotalNum;j++)
	{	for(k=0;k<obsrec[j].epohead.iSATNUM;k++)
		{	int iPrn=obsrec[j].eporec[k].iPRN;			
			SatObsRec[iPrn-1].PObsEpoRec[p[iPrn-1]]= &obsrec[j].eporec[k];
			SatObsRec[iPrn-1].PObsEpoRec[p[iPrn-1]]->iEpoNum=j;
			p[iPrn-1]++;
		}
	}
	for (i=0;i<MAXNAVSAT;i++)
	{	if (SatObsRec[i].PObsEpoRec[0])
		{SatObsRec[i].PObsEpoRec[0]->iCycleFlag=1;
		for (j=1;j<p[i];j++)
			if((SatObsRec[i].PObsEpoRec[j]->iEpoNum-SatObsRec[i].PObsEpoRec[j-1]->iEpoNum)>1)
				SatObsRec[i].PObsEpoRec[j]->iCycleFlag=1;//   A new satellite occurs   Marks 1	
		}		
	}	
	return 1;
}

BOOL PhaSmoPseudo(SATOBSREC *SatObsRec,int *obstype,int *iEpoNumofSat)
{	long double lWeight;
	int iNumofArc;
	for(int i=0;i<31;i++)
	{	if(i==14)
			int csl=i;
		for(int j=0;j<iEpoNumofSat[i];j++)
		if(SatObsRec[i].PObsEpoRec[j])
		{	if(SatObsRec[i].PObsEpoRec[j]->iCycleFlag<0)
			{	if(!SatObsRec[i].PObsEpoRec[j]->lOBS[obstype[P1]])
					SatObsRec[i].PObsEpoRec[j]->lOBS[obstype[P1]]=SatObsRec[i].PObsEpoRec[j]->lOBS[obstype[C1]];
				SatObsRec[i].PObsEpoRec[j]->lPhaSmoPse=SatObsRec[i].PObsEpoRec[j]->lOBS[obstype[P1]];	
				continue;
			}
			if(SatObsRec[i].PObsEpoRec[j]->iCycleFlag>0)
			{    lWeight=1;iNumofArc=0;
				 SatObsRec[i].PObsEpoRec[j]->lPhaSmoPse=SatObsRec[i].PObsEpoRec[j]->lOBS[obstype[P1]];
				 iNumofArc++;continue;
			}
			iNumofArc++;
			lWeight=1.0/double(iNumofArc);
			SatObsRec[i].PObsEpoRec[j]->lPhaSmoPse=lWeight*(SatObsRec[i].PObsEpoRec[j]->lOBS[obstype[P1]])
				+(1-lWeight)*(SatObsRec[i].PObsEpoRec[j-1]->lPhaSmoPse-
				(WAVELENGTH1)*(SatObsRec[i].PObsEpoRec[j-1]->lOBS[obstype[L1]]
				-SatObsRec[i].PObsEpoRec[j]->lOBS[obstype[L1]]));
			//cout<<SatObsRec[i].PObsEpoRec[j]->lOBS[obstype[P1]]<<"  "
			//<<SatObsRec[i].PObsEpoRec[j]->lPhaSmoPse<<endl;
		}
	}
	return TRUE;
}


int  SatObsRecDesTroy(SATOBSREC *SatObsRec)
{
	for (int i=0;i<31;i++)
	{
		delete[]SatObsRec[i].PObsEpoRec;
 	}
	delete[]SatObsRec;
	return 1;
}