errorcorrection.cpp

GPS 定位授时源码

#include "StdAfx.h"
#include "ErrorCorrection.h"
#include "math.h"
#include "Matrixcsl.h"



/*
int CORRECTCLK(int num,long double *rawclk,long double *recclk)
{   //detect and correct the clock jump 
 	//have been corrected at10.17 must be perfectly

	//recclk=new double[num];
	BOOL isClockJump=FALSE;
	int i,j;
	for(i=0;i<num;i++)
		recclk[i]=rawclk[i]*1e+9;
	//Firstly transformed into m's unit

	long double lClkTem,
				lDeltaSquareOld,
				lDelta,
				lDeltaSquareNew;	
	lDeltaSquareOld=pow(RMS,2);
	// not very well for the bad first two epoches
	//a const number is not suitable for the reality	
	int iArcNum=2;
	for(i=2;i<num;i++)
	{	iArcNum++;
		long double lDif=recclk[i]-recclk[i-1];
		lClkTem=recclk[i-1]+lDif/iArcNum;
		lDeltaSquareNew=lDeltaSquareOld+(pow(lDif,2)-lDeltaSquareOld)/iArcNum;
		lDelta=pow(lDeltaSquareOld,0.5);		
		if(fabs(lDif)<2.0*lDelta)
		{	//recclk[i]=lClkTem;
			lDeltaSquareOld=lDeltaSquareNew;
			continue;
		}
		if(abs(lDif>2*lDelta))		
		{	if(fabs((recclk[i+1]-recclk[i-1]))<2.0*lDelta)
			{
				recclk[i]=recclk[i-1];
				isClockJump=TRUE;
			}
			else
			{	for (j=i;j<num;j++)	
					recclk[j]-=lDif;
				iArcNum=1;
				//lDeltaSquareOld=10;
				isClockJump=TRUE;
			}
		}
	}
	for(i=0;i<num;i++)
		recclk[i]*=1e-9;
	//s to ns
	if(isClockJump)
		return 0;
	return 1;
}*/


int  CORRECTCLK(int num,long double *rawclk,long double *recclk)
{   //detect and correct the clock jump 
	//have been corrected at10.17 must be perfectly
	
	BOOL isClockJump=FALSE;
	for(int i=0;i<num;i++)
		recclk[i]=rawclk[i];
	for( i=1;i<num;i++)
	{
		long double lDif=recclk[i]-recclk[i-1];			
		if(fabs(fabs(lDif)-0.001)<0.0001)
		{	isClockJump=TRUE;
			for (int j=i;j<num;j++)	
			recclk[j]-=lDif;
		}				
	}

	if(isClockJump)
		return 0;
	return 1;
}


double RELATIVECORRECTION(SATPOS &satpos)
{
	double relcor;
	relcor=2*(satpos.xyz.lX*satpos.Dotxyz.lX+satpos.xyz.lY*satpos.Dotxyz.lY+satpos.xyz.lZ*satpos.Dotxyz.lZ)/VC;
	return relcor;
}

void Earthrotation(double tao,SATPOS * satpos)
{
	 double omiga       = PI*2.0/(3600*24.00);
	 double alfa        = omiga*tao;
	 double cosa        = cos(alfa);
	 double sina        = sin(alfa);
	 double p			=  satpos->xyz.lX*cosa+satpos->xyz.lY*sina;
	 double q			= -satpos->xyz.lX*sina+satpos->xyz.lY*cosa;
	 satpos->xyz.lX		= p;
	 satpos->xyz.lY		= q;	 
}


long double SIMPLEHOPFIELD(int MODEL,long double ELEV,long double P4,long double T4,long double RH4,BLH sblh)	
{//RH4   相对湿度    H4 高度
	long double DR=0.0;
	double BCOR[6]={1.156,1.006,0.874,0.757,0.654,0.563};
	long double H4=sblh.lH;     //orthogonal height using the geoid
	if(H4<0)
		H4=0;		// to avoid the case of -6378km when the geocordinate is (0,0,0)
	
	T4=T4+273.15;   //  centigrade to K (absolute temperature)  
	
	if(RH4>100.0)
		RH4=100.0;

	double E=RH4/100.0*exp(-37.2465+0.213166*T4-0.000256908*T4*T4);   
	// WATER VAPOR PRESSURE

	if(MODEL==1)
	{ 
		ELEV=ELEV*PI/180.0;
		// 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 B=BCOR[I]+(BCOR[I+1]-BCOR[I])*(HL-HREF);
  
		//TROPOSPHERIC DISTANCE CORRECTION DR
		DR = 2.277E-3*(1+0.0026*cos(2*sblh.lB)+0.00028*HL)*(P4+(1255/T4+0.05)*E-B*pow(tan(ELEV),-2))/sin(ELEV);  
		
		//********************************************************************* 
		// standard SAASTAMOINEN //2005-6-13 added
		//hydro zpd only
		//*zhd = 0.002277*(1+0.0026 * cos( 2*sblh.B ) + 0.00028*HL )*P4;
		//wet zpd
		// *zwd = 0.002277*(1255/T4+0.05)*E;
		return DR;  
	}

	if(MODEL==2)
	{
		// MODEL 2: MODIFIED HOPFIELD
		double ALPHA[10],N[2],H[2];
		N[0]=0.776E-4*P4/T4;
		N[1]=0.373*E/T4/T4;
		H[0]=40.136+0.14872*(T4-273.16);
		H[1]=11.0;

		// ELEVATION ANGLE  in 	radian
		double THETA=ELEV*PI/180.0;   
		double AE=6378.0+H4/1000.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.0E3*DRI*N[i];
		}
	  
	    return DR;
  	}
  
	if(MODEL==3)
	{
		// MODEL 3: SIMPLIFIED HOPFIELD
		long double DR=0;
		double RH=(RH4>100)?100:RH4;
		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);
		double E2=pow(ELEV,2);
		DR =KD/sin(sqrt(E2+6.25)*PI/180.0)+KW/sin(sqrt(E2+2.25)*PI/180.0);
		return DR;
	}	
	else return DR;
 }


int LEASTSQUAREAJUST(long double **X,long double **B,int iRow ,int iLine,int iXOrder,long double **L,long double **P,long double **Qx)
{	
	long double **plBT     =TwoArrayAlloc(iLine,iRow);
	long double **plBTP    =TwoArrayAlloc(iLine,iRow);
	long double **plBTB    =TwoArrayAlloc(iLine,iLine);
	// the only purpose of BTP is to figure out the Tdop
	long double **plBTPB   =TwoArrayAlloc(iLine,iLine);
	long double **plInvBTPB=TwoArrayAlloc(iLine,iLine);
	long double **plBTPL   =TwoArrayAlloc(iLine,iXOrder);
	
	AtMatrics(B,iRow,iLine,plBT);
	AMBProduct(plBT,iLine,iRow,P,iRow,plBTP);
	AMBProduct(plBTP,iLine,iRow,B,iLine,plBTPB);
	InvMatrix(iLine,plBTPB,plInvBTPB);

	AMBProduct(plBTP,iLine,iRow,L,iXOrder,plBTPL);
	AMBProduct(plInvBTPB,iLine,iLine,plBTPL,iXOrder,X);
	
	AMBProduct(plBT,iLine,iRow,B,iLine,plBTB);
	InvMatrix(iLine,plBTB,Qx);
	//	AMBProduct(Qx,iLine,iLine,plBTPL,iXOrder,X);
	

	TwoArrayFree(plBT);
	TwoArrayFree(plBTP);
	TwoArrayFree(plBTPB);
	TwoArrayFree(plBTB);
	TwoArrayFree(plInvBTPB);
	TwoArrayFree(plBTPL);
	return 1;
}

int LEASTSQUAREAJUST(long double **X,long double **B,int iRow ,int iLine,long double **L)
{	
	long double **plBT    =TwoArrayAlloc(iLine,iRow);
	long double **plBTB   =TwoArrayAlloc(iLine,iLine);
	long double **plInvBTB=TwoArrayAlloc(iLine,iLine);
	long double **plBTL   =TwoArrayAlloc(iLine,1);
	
	AtMatrics(B,iRow,iLine,plBT);
	AMBProduct(plBT,iLine,iRow,B,iLine,plBTB);
	InvMatrix(iLine,plBTB,plInvBTB);
	AMBProduct(plBT,iLine,iRow,L,1,plBTL);
	AMBProduct(plInvBTB,iLine,iLine,plBTL,1,X);
	
	TwoArrayFree(plBT);
	TwoArrayFree(plBTB);
	TwoArrayFree(plInvBTB);
	TwoArrayFree(plBTL);
	return 1;
}

int RECANTCORRECTION(long double &X1,long double &Y1,long double &Z1,long double X2,long double Y2,long double Z2)
{
	X1+=X2;Y1+=Y2;Z1+=Z2;
	return 1;
}