getposotion.cpp

读取GPS的Rinex观测文件

	int m_S1=-1;   int m_S2=-1;
	int m_D1=-1;   int m_T1=-1;
	int m_L5=-1;
    
	//确定使用伪距所在的位置
	for(int type=0;type<gmoh.obs_type_number;type++)
	{
	     if(gmoh.obs_type_list[type]==1)
		{
			m_L1=type;	
		}
		 if(gmoh.obs_type_list[type]==2)
		{
			m_L2=type;		
		}
		 if(gmoh.obs_type_list[type]==3)
		{
			m_p1=type;		
		}
		 if(gmoh.obs_type_list[type]==4)
		{
			m_p2=type;
		}
		 if(gmoh.obs_type_list[type]==5)
		{
			m_S1=type;	
		}
		 if(gmoh.obs_type_list[type]==6)
		{
			m_S2=type;	
		}
		 if(gmoh.obs_type_list[type]==7)
		{
			m_value=type;
			flag=1;		
		}
		 if(gmoh.obs_type_list[type]==8)
		{
			m_D1=type;	
		}
		 if(gmoh.obs_type_list[type]==9)
		{
			m_T1=type;	
		}
		 if(gmoh.obs_type_list[type]==9)
		{
			m_L5=type;	
		}
	}


	//确定能使用的卫星数目
    int bz=0,vp=0,vh=0,t=0;
	int sat_value[_MAX_CNT_OBS_NUM]={0};  //卫星数
	int sat_pos[_MAX_CNT_OBS_NUM]={0};
	int PN;

	int pre=0;//判断伪距观测值是用P1,P2还是C1,为0时表示用P1,P2.

	if(m_p1!=-1&&m_p2!=-1)//如果有P1,P2就采用双频改正模型来给正电离层的延迟
	{
		for(int j=0;j<(*pgmor)->num_sat;j++)//遍历所有卫星
		{
			PN=(*pgmor)->prn_lst[j];
			for(pgmnr=navRecord.begin();pgmnr!=navRecord.end();pgmnr++)
			{
				if(PN==(*pgmnr)->PRN&&(*pgmor)->value[j][m_p1]!=0&&(*pgmor)->value[j][m_p2]!=0)//判断该卫星是否可用
				{
					sat_value[vp]=PN;
					vp++;
					sat_pos[vh]=j;
					vh++;
					bz=1;
					break;
				}

			}

		if(bz==0)
			t++;
		else
			bz=0;		
		}
	}
	else//如果没有P1或P2，就直接用C1计算伪距，忽略电离层影响
	{   
		pre=1;
		for(int j=0;j<(*pgmor)->num_sat;j++)
		{
			PN=(*pgmor)->prn_lst[j];
			for(pgmnr=navRecord.begin();pgmnr!=navRecord.end();pgmnr++)
			{
				if(PN==(*pgmnr)->PRN&&(*pgmor)->value[j][m_value]!=0)
				{
					sat_value[vp]=PN;
					vp++;
					sat_pos[vh]=j;
					vh++;
					bz=1;
					break;
				}

			}
			if(bz==0)
				t++;
			else
				bz=0;
		}

	}
	 
    int sat_valuesum=(*pgmor)->num_sat-t;//参与解算的卫星数
	int len=sat_valuesum;
	////////////////////////////////////////////////////////////////
   
	SatVel satv;//卫星的速度
	GMNREC m_nearRec;//最近历元
	double dxx=0,dyy=0,dzz=0;
    double D,Pdot;
	double Xsitev=0,Ysitev=0,Zsitev=0;//测站速度
////////////////////////////////////////////////////////

	if(sat_valuesum>=4)
	{
		double X0,Y0,Z0,DetTi;
		double dx,dy,dz,dt;
		X0=gmoh.approx_pos.x;
		Y0=gmoh.approx_pos.y;
		Z0=gmoh.approx_pos.z;

        if(X0==0)
		{
		    X0=1000000;
		    Y0=1000000;
		    Z0=1000000;
		}
		DetTi=0;//初始化值
        Matrix B(len,4),W(len,1),Result(4,1),Q(4,4);
		Matrix BB(len,4),WW(len,1),RResult(4,1);
        
		double m_dop=0;
		double X,Y,Z;
		double dett2,dett1;
		double tbias,detj,R=20000000; /////////////////////////////////////////

	   	 COMMONTIME   tj;//信号发射时间
		 tj.year=(*pgmor)->epoch.year;
		 tj.month=(*pgmor)->epoch.month;
		 tj.day=(*pgmor)->epoch.day;
		 tj.hour=(*pgmor)->epoch.hour;
		 tj.minute=(*pgmor)->epoch.minute;
		 tj.second=(*pgmor)->epoch.second;

		 
		 JULIANDAY tjJ;//发射时间
		 JULIANDAY epo;//历元时间

		///////////////////////////////
		 //用来计算对流层延迟
		CRDCARTESIAN crdSite;//测站坐标
		CRDCARTESIAN crdSat;//卫星坐标
   
		CRDCARTESIAN pcrdOrb1;

		int PRN;//有效卫星的卫星号
		double p0;//伪距
	    int pos;//有效卫星在观测星历中的位置
		////////////////////////////////////////////////////
		do{
			 ncount++;
			 crdSite.x =X0;
			 crdSite.y=Y0;
			 crdSite.z=Z0;
			 for(int i=0;i<len;i++)//  参与解算的卫星 数目
			 {
				
				 PRN=sat_value[i];
				 //初始化卫星钟差
				 GetSVClkBias(navRecord,PRN,&(*pgmor)->epoch,&tbias,&detj);//卫星发射时刻改正
	//函数原型如下
	//			 GetSVClkBias(list<PGMNREC>& navRecord,int& nPRN,PCOMMONTIME pctEpoch,
	//				  double* pdSVClkBias,double *detj);

				 pos=sat_pos[i];
				
				 if(pre==0)//用P1,P2                                                 两种电离层改正方法
					 p0=(*pgmor)->value[pos][m_p1]*2.54573
						 -(*pgmor)->value[pos][m_p2]*1.54573;
				 else//用C1
					 p0=(*pgmor)->value[pos][m_value];
			
				 dett2=p0/c;//传播时间
           
				 do
				 {
					
					 dett1=dett2;
					 //时间计算
					 CommonTimeToJulianDay(&(*pgmor)->epoch, &epo);
					 SetTimeDelta (&tjJ, &epo, (-tbias-dett1));
					 JulianDayToCommonTime (&tjJ, &tj);
                

					 //计算卫星在笛卡尔坐标系中的位置
					 GetOrbNClk(navRecord,PRN,&tj,&pcrdOrb1);
	//				 void GetOrbNClk(list<PGMNREC>& navRecord,int& nPRN,PCOMMONTIME pctEpoch, 
	//				PCRDCARTESIAN pcrdOrb);
					 //计算卫星C/A码信号发射时刻的改正
					 GetSVClkBias(navRecord,PRN,&tj,&tbias,&detj);

					//地球自转的改正
					 X=cos(dett1*we)*pcrdOrb1.x+sin(dett1*we)*pcrdOrb1.y;
					 Y=-sin(dett1*we)*pcrdOrb1.x+cos(dett1*we)*pcrdOrb1.y;
					 Z=pcrdOrb1.z;

					 crdSat.x=X;
					 crdSat.y=Y;
					 crdSat.z=Z;

					 R=sqrt((X-X0)*(X-X0)+(Y-Y0)*(Y-Y0)+(Z-Z0)*(Z-Z0));//计算距离
			  
					 dett2=R/c;//传播时间

				}while(fabs(dett2-dett1)>1e-9);

				 B(i,0)=(X0-X)/R;
				 B(i,1)=(Y0-Y)/R;
				 B(i,2)=(Z0-Z)/R;
				 B(i,3)=1;
         
				 toprelay=GetTropDelay(&crdSite,&crdSat);//对流层延迟

				 W(i,0)=p0-R+c*detj-c*DetTi +toprelay;
			
					
	//////////////////////////////////////////////////////////////
				 if(flag==1)//是否进行速度计算
				 {
				  GetSatVelocity(navRecord,PRN,&tj,&satv);//计算卫星的速度
				  pgmnr=GetBestGMNREC(navRecord,PRN,&tj);//用于得到a1
				  dxx=X-X0;
				  dyy=Y-Y0;
				  dzz=Z-Z0;

				 BB(i,0)=-dxx/R;
				 BB(i,1)=-dyy/R;
				 BB(i,2)=-dzz/R;
				 BB(i,3)=1;

				 D=(dxx*satv.xv+dyy*satv.yv+dzz*satv.zv)/R;
				 Pdot=(*pgmor)->value[pos][m_D1]*c/f1;
             
				// if(fabs(Pdot-D)>50)
				//	 Pdot=-Pdot;

				  WW(i,0)=-Pdot-D+c*(*pgmnr)->ClkDrift;
				 }
	///////////////////////////////////////////////////////////////
			 
			 }
     
      
			Result=!(~B*B)*~B*W;
			Q=!(~B*B);

			dx=Result(0,0);
			dy=Result(1,0);
			dz=Result(2,0);
			dt=Result(3,0);
			X0+=dx;
			Y0+=dy;
			Z0+=dz;
			DetTi+=dt/c;

			if(ncount>10)//超过10 次以上的迭代视为失败
			{	
//				return false;
				break;
			}
		   }while(fabs(dx)>0.1||fabs(dy)>0.1||fabs(dz)>0.1);

		for(int q=0;q<3;q++)
			m_dop+=Q(q,q);

		if(flag==1)
		{
				RResult=!(~BB*BB)*~BB*WW;
				 Xsitev=RResult(0,0);
				 Ysitev=RResult(1,0);
				 Zsitev=RResult(2,0);
		}
				 
      presult->sat_num=len;
      presult->clk_bias=DetTi;

	  presult->crd.x=X0;
	  presult->crd.y=Y0;
	  presult->crd.z=Z0;

	  presult->epoch.year=(*pgmor)->epoch.year;
	  presult->epoch.month=(*pgmor)->epoch.month;
	  presult->epoch.day=(*pgmor)->epoch.day;
	  presult->epoch.hour=(*pgmor)->epoch.hour;
	  presult->epoch.minute=(*pgmor)->epoch.minute;
	  presult->epoch.second=(*pgmor)->epoch.second;

	  presult->PDOP=sqrt(m_dop);
      
	  presult->m_sitev.xv=Xsitev;
	  presult->m_sitev.yv=Ysitev;
	  presult->m_sitev.zv=Zsitev;
      presult->flag=flag;   
	  
//	   return true;
	
	}

	
//	else
//	{
//		return false;
//	}
  

}