pp.cpp

卫星单点定位程序

#include "pp.h"

//单点定位（单个历元）
void PPOne (PGMOREC pRec,				//观测历元的观测值
            PGMO pGMO,					//观测数据
            PGMN pGMN,					//导航电文数据
			SP3* pSP3,					//SP3精密星历
            CRDCARTESIAN crdInit,		//测站点坐标的初始值（WGS-84）
            double rcvr_clk_biasInit,   //钟差的初始值
            PPPONERESULT pResult)		//计算结果
{

    int i;
    int useless = 0;						//不合格的观测值数目
	Matrix X0(4, 1);                         //坐标和钟差的初始值(X, Y, Z, tr)
    Matrix X(4, 1);                          //计算结果
    Matrix B(pRec->sat_num, 4);             //条件方程组的系数
    Matrix L(pRec->sat_num, 1);             //观测值
    Matrix L0(pRec->sat_num, 1);            //
    Matrix Lt;            //观测值的平差值
    Matrix l;             //误差方程组的常数值
    Matrix N;
    Matrix v;             //观测值的改正值
    Matrix x;                         //参数的近似值
    Matrix xt;                        //参数的平差值
    Matrix w;							//
	Matrix Q;

    int C1_sn = 0;              // C1 码观测值在单颗卫星观测值中的序号

    //计算C1 观测值的序号
    for (i=0; i<pGMO->hdr.obs_type_number; i++) {
        if ((pGMO->hdr.obs_type[i][0] == 'C')
            && (pGMO->hdr.obs_type[i][1] == '1'))   {
            C1_sn = i;
            break;
        }
    }

    //给定待定点的初始(近似)坐标(X Y Z)和初始(近似)的接收机钟差(tr)
    X0(0, 0) = crdInit.x;
    X0(1, 0) = crdInit.y;
    X0(2, 0) = crdInit.z;
    X0(3, 0) = rcvr_clk_biasInit;

    //计算每颗卫星的伪距观测方程
    for (i=0; i<pRec->sat_num; i++) {
		double R = 0;						//测站初始位置与信号发射时刻卫星位置之间的距离
        double tt = 0;                      //信号传播时间
		double tt0 = 100;				//信号传播时间循环初始值
		double dts = 0;					//相应卫星钟差
        COMMONTIME ctTs;                //信号发射时刻
        CRDCARTESIAN crdSatSend;        //卫星在发射时刻在地心地固系的位置
        CRDCARTESIAN crdSatIncept;      //卫星在接收时刻在地心地固系的位置  
		const GPSOBS* ps									//伪距
			= &(pRec->obs[C1_sn + i * pGMO->hdr.obs_type_number]);
		const PCOMMONTIME ctTr = &pRec->epoch;		//当前历元时刻
		double tmp = 0;
		double TropDelay = 0;				//对流层延迟
		double elevation = 0;				//卫星高度角

        //卫星在接收时刻在地心地固系的位置及卫星钟差
        GetOrbNClk (pGMN, pRec->sat_list[i], &pRec->epoch, &crdSatIncept, &dts);
//		GetOrbNClkSP3 (pSP3, pRec->sat_list[i], &pRec->epoch, &crdSatIncept, &dts);

		//计算气象改正以及卫星高度角，剔除高度角小于 10°的卫星
		elevation = GetTropDelay(&TropDelay, NULL, &crdInit, &crdSatIncept);
		if ((elevation - LEAST_ELEVATION) < 10e-8)	{
			useless++;
			continue;
		}	
		//计算传播时间
		for ( ; fabs(tt0 - tt) > 10e-10; )	{
			double alpha;				//信号传播过程中卫星转过的角度
			JULIANDAY jdTr;				//当前历元时刻的儒略历表示
			JULIANDAY jdTs;				//信号发射时刻的儒略历表示

			tt0 = tt;
			tt = ps->value / c - rcvr_clk_biasInit + dts;
			CommonTimeToJulianDay(ctTr, &jdTr);
			SetTimeDelta(&jdTs, &jdTr, -tt);
			JulianDayToCommonTime(&jdTs, &ctTs);

			GetOrbNClk (pGMN, pRec->sat_list[i], &ctTs, &crdSatSend, &dts);
//			GetOrbNClkSP3 (pSP3, pRec->sat_list[i], &ctTs, &crdSatSend, &dts);
			
			alpha = 7.2921151467e-5 * tt;
			//通过绕z 轴的旋转变换，计算卫星信号发射卫星位置在接收时刻的地固系下的坐标。
			crdSatIncept.x = cos(alpha) * crdSatSend.x + sin(alpha) * crdSatSend.y;
			crdSatIncept.y = -sin(alpha) * crdSatSend.x + cos(alpha) * crdSatSend.y;
			crdSatIncept.z = crdSatSend.z;
			R = sqrt(pow(crdSatIncept.x - crdInit.x, 2)
				+ pow(crdSatIncept.y - crdInit.y, 2)
				+ pow(crdSatIncept.z - crdInit.z, 2));
			tt = R / c;
        }
		
		//误差方程系数
		B(i - useless, 0) = (crdSatIncept.x - crdInit.x) / R;
		B(i - useless, 1) = (crdSatIncept.y - crdInit.y) / R;
		B(i - useless, 2) = (crdSatIncept.z - crdInit.z) / R;
		B(i - useless, 3) = 1.0;
		
		//观测值
		L(i - useless, 0) = ps->value - TropDelay;
//		printf("%f\t%f\n",TropDelay,elevation);

		//观测值的改正值
		L0(i - useless, 0) = R - c * (dts - rcvr_clk_biasInit); 
    }
	B.m = pRec->sat_num - useless;
	L.m = pRec->sat_num - useless;
	L0.m = pRec->sat_num - useless;
    l = L - L0;         //l=L-(B*X0+d)=L-L0

    N = ~B * B;
//B.display();
//L.display();
//L0.display();
//l.display();
    w = ~B * l;
    xt = !N * w;
    v = B * xt - l;
    Lt = L + v;
    X = X0 - xt;
	X(3, 0) = X0(3, 0) + xt(3, 0) / c;
	
//	N.display();
//	X0.display();
//	xt.display();
//	v.display();

	//如果计算精度太低则采用递归，否则给结果赋值
	if (fabs(X(3,0) - X0(3,0)) > 10e-10)	{
		crdInit.x = X(0,0);
		crdInit.y = X(1,0);
		crdInit.z = X(2,0);
		rcvr_clk_biasInit = X(3,0);
		PPOne(pRec, pGMO, pGMN, pSP3, crdInit, rcvr_clk_biasInit, pResult);
	}
	else {
		memcpy(&pResult->epoch, &pRec->epoch, sizeof(COMMONTIME)); 
		pResult->crd.x = X(0,0);
		pResult->crd.y = X(1,0);
		pResult->crd.z = X(2,0);
		pResult->clk_bias = X(3,0); 
		pResult->sat_num = pRec->sat_num - useless;
		Q = !N;
		pResult->residual = sqrt((~v * v)(0, 0) / (pRec->sat_num - useless - 4));
		pResult->PDOP = sqrt(Q(0, 0) + Q(1, 1) + Q(2, 2));
	}
}


//单点定位（所有历元）
PPPRESULT PP (PGMO pGMO,    //观测数据
              PGMN pGMN,    //导航电文数据
			  SP3* pSP3)	//SP3精密星历
{
    PPPRESULT pResult = NULL;
	PPONERESULT PPOneResult;
	int i;
	
	pResult = new PPRESULT;
	pResult->result = new PPONERESULT[pGMO->hdr.epoch_number];
	pResult->epoch_num = pGMO->hdr.epoch_number; 
	PPOneResult.crd.x = pGMO->hdr.approx_pos.x;
	PPOneResult.crd.y = pGMO->hdr.approx_pos.y;
	PPOneResult.crd.z = pGMO->hdr.approx_pos.z;
	PPOneResult.clk_bias = 0;
	for (i = 0; i < pGMO->hdr.epoch_number; i++)	{
		printf("%hhd-%hhd-%2.0f\t%4d\n",pGMO->record[i].epoch.hour,
			pGMO->record[i].epoch.minute, pGMO->record[i].epoch.second, i);
		PPOne(&pGMO->record[i], pGMO, pGMN, pSP3, PPOneResult.crd,
			PPOneResult.clk_bias, &pResult->result[i]);
		PPOneResult.crd.x = pResult->result[i].crd.x;
		PPOneResult.crd.y = pResult->result[i].crd.y;
		PPOneResult.crd.z = pResult->result[i].crd.z;
	}
    return pResult;
}