void SingleSys(Position spp,DdData& dddataPre,DdData& dddataCurr,SdData& lastSdData,ObsEpochData& roverData1,ObsEpochData& baseData1, SppCtrl sppctrl,DdCtrl ddctrl, DdAmbInfo& ambinfo,DdAmbInfo& preambinfo,SppInfo sppinfo,SppInfo baseInfo,DdObsInfo ddobsinfo1, BroadEphHeader brdephheader,BroadEphData* broadephdata,BroadEphDataGlo* gloeph,int nSate,int nSateGlo, ObsHeader obsHeader1,ObsEpochData epochData1,ObsHeader obsHeader2,ObsEpochData epochData2, double* baseCrd,double* roverCrd,int& nEpoch, int sysidUse,math::matrix<double>* NormEqu) { lastSdData.ZeroElem();roverData1.ZeroElem(); baseData1.ZeroElem(); sppinfo.ZeroElem(); baseInfo.ZeroElem(); if (sysidUse==1 || sysidUse==3 ||sysidUse==5) { spp.StandPosition(brdephheader,broadephdata,epochData1,nSate,sppctrl,sppinfo,roverData1); spp.baseStn(baseInfo,broadephdata,epochData2,baseData1,nSate,sppctrl); } else if(sysidUse==2) { //spp.StandPosition(brdephheader,gloeph,epochData1,nSateGlo,sppctrl,sppinfoGlo,roverData);//Glo //spp.baseStn(baseInfoGlo,gloeph,epochData2,baseData,nSateGlo,sppctrl);//glo } if(Norm(roverCrd,3)>0.0) PtrEqual(roverCrd,sppinfo.recPos,3); int refPrn=0; ddobsinfo1.ZeroEle(); int tprn=0; refPrn=spp.SelectRefSate(baseInfo,sppinfo,5.0,baseData1,roverData1,lastSdData,ddobsinfo1,0,tprn); int refpos; refpos=GetPos(lastSdData.prn,refPrn,lastSdData.satnum); dddataCurr.ZeroElem(); ambinfo.ZeroElem(); spp.DoubleDiff(refPrn,refpos,lastSdData,dddataCurr); if(dddataCurr.pairNum<4) exit(1); //DdData ts=dddataCurr; /* -----------------------------------------end data preparation part ---------------------------------------*/ dddataCurr=spp.ComObsPhsCod(ddctrl,ddobsinfo1,ambinfo,dddataCurr); //if(nEpoch>1) spp.PassPreAmb(preambinfo,ambinfo,ddctrl.PhsTypeNo()); /* -----------------------------------------end cycle slip---------------------------------------- */ int obsNum =ddobsinfo1.SumCod()+ddobsinfo1.SumPhs();//the number of all obs in one system at current epoch int phsNum =ddobsinfo1.SumPhs(); int ionoNum=dddataCurr.pairNum; int ambNum=ambinfo.SumUnfix(); //if(nEpoch>1)ambNum=preambinfo.SumUnfix(); if(ambNum==0) ambNum=1; /* equation part */ math::matrix<double> DesMatPos(obsNum,3); math::matrix<double> DesMatAmb(obsNum,ambNum); math::matrix<double> Weight(obsNum,obsNum); math::matrix<double> L(obsNum,1); math::matrix<double> DesMatTrop(obsNum,1); math::matrix<double> DesMatIono(obsNum,obsNum); spp.FormDdErrEq(DesMatPos,DesMatTrop,DesMatIono,DesMatAmb,L,dddataCurr,ddctrl,ambinfo,ddobsinfo1); Weight=spp.FormWeightVc(ddctrl,dddataCurr,ddobsinfo1); NormEqu[0]=~DesMatPos*Weight*(DesMatPos); NormEqu[1]=~DesMatPos*Weight*(DesMatAmb); NormEqu[2]=~DesMatAmb*Weight*(DesMatAmb); NormEqu[3]=~DesMatPos*Weight*L; NormEqu[4]=~DesMatAmb*Weight*L; }
/*for QXWZ, 2015.12*/ void ProcessSingleFixPos(int sysidUse) { ProcessRinex readdata; CString CommadFile =_T("CommandFile.txt"); //control block DdData dddataPre; DdData dddataCurr; SdData lastSdData; ObsEpochData roverData,baseData; //single system DdObsInfo ddobsinfo,preddobsinfo; DdAmbInfo ambinfo,preambinfo; int numCod=0,numPhs=0; math::matrix<double> prenorm[5]; double dt; SppCtrl sppctrl; Position spp; SppInfo sppinfo,baseInfo; SppInfoGlo sppinfoGlo,baseInfoGlo; DdCtrl ddctrl; InPutFileSet inputfile; double baseCrd[3],roverCrd[3]; DdCtrl ddCtrlPtr[MAXSYS]; ReadCommFile(ddCtrlPtr,inputfile,_T("CommandFile.txt"),baseCrd,roverCrd); inputfile.CheckPath(); ddctrl=ddCtrlPtr[sysidUse-1]; sppctrl.sysid=ddctrl.sysid; //read broadcast eph CString NFileName=inputfile.fileEph[0]; BroadEphHeader brdephheader; BroadEphData* broadephdata=new BroadEphData[MAXEPHNUM]; BroadEphDataGlo* gloeph=new BroadEphDataGlo[MAXEPHNUM_GLO]; int nSate =0; int nSateGlo =0; readdata.ReadBroadEph(NFileName, brdephheader,broadephdata,gloeph, nSate,nSateGlo); //Rover station ObsHeader obsHeader1; ObsEpochData epochData1; CString OFileName1 =inputfile.fileRover[0]; CStdioFile Ofile1; Ofile1.Open(OFileName1,CFile::modeRead); CString line1; readdata.ReadObsHeader(Ofile1,line1,obsHeader1); //Base station for (int i=0;i<3;i++) baseInfo.recPos[i]=baseCrd[i]; ObsHeader obsHeader2; ObsEpochData epochData2; CString OFileName2 =inputfile.fileBase[0]; CStdioFile Ofile2; Ofile2.Open(OFileName2,CFile::modeRead); CString line2; readdata.ReadObsHeader(Ofile2,line2,obsHeader2); int nEpoch1=0,nEpoch2=0; int eventFlag1=0,eventFlag2=0; int nEpoch=0; //output to file fstream fout; fout.open("QXWZ.txt",ios::out); math::matrix<double>Npre_amb,Upre_amb; int cycleslip_index_L1[MAXNUMSATE]; int cycleslip_index_L2[MAXNUMSATE]; double ptrMapPre[MAXNUMSATE]; double ptrMapCur[MAXNUMSATE]; int tprn=0; math::matrix<double> PreVcmAmb; math::matrix<double> PreSoluAmb; math::matrix<double>* CurVcm=new math::matrix<double>[3]; math::matrix<double>* CurSolu=new math::matrix<double>[2]; math::matrix<double>Nneq; math::matrix<double>Uneq; while (eventFlag1!=10 && eventFlag2!=10) { epochData1.ZeroElem(); epochData2.ZeroElem(); epochData1=readdata.GetEpochData(Ofile1,line1,obsHeader1,nEpoch1,eventFlag1); epochData1.Sort(); epochData2=readdata.GetEpochData(Ofile2,line2,obsHeader2,nEpoch2,eventFlag2); epochData2.Sort(); dt=(epochData1.week-epochData2.week)*7*86400.0+(epochData1.sec-epochData2.sec); if (epochData1.sec<260112.0) { //continue; } if (fabs(dt)<0.5) { clock_t start,finish; double dura=0.0; nEpoch++; start=clock(); lastSdData.ZeroElem(); roverData.ZeroElem(); baseData.ZeroElem(); sppinfo.ZeroElem(); baseInfo.ZeroElem(); if (sysidUse==1 || sysidUse==3 ||sysidUse==5) { spp.StandPosition(brdephheader,broadephdata,epochData1,nSate,sppctrl,sppinfo,roverData); spp.baseStn(baseInfo,broadephdata,epochData2,baseData,nSate,sppctrl); } else if(sysidUse==2) { spp.StandPosition(brdephheader,gloeph,epochData1,nSateGlo,sppctrl,sppinfoGlo,roverData);//Glo spp.baseStn(baseInfoGlo,gloeph,epochData2,baseData,nSateGlo,sppctrl);//glo } if(Norm(roverCrd,3)>0.0) PtrEqual(roverCrd,sppinfo.recPos,3); int refPrn=0; ddobsinfo.ZeroEle(); refPrn=spp.SelectRefSate(baseInfo,sppinfo,5.0,baseData,roverData,lastSdData,ddobsinfo,1,tprn); int refpos; refpos=GetPos(lastSdData.prn,refPrn,lastSdData.satnum); dddataCurr.ZeroElem(); ambinfo.ZeroElem(); spp.DoubleDiff(refPrn,refpos,lastSdData,dddataCurr); if(dddataCurr.pairNum<4) continue;//exit(1) DdData ts=dddataCurr; /* -----------------------------------------end data preparation part ---------------------------------------*/ dddataCurr=spp.ComObsPhsCod(ddctrl,ddobsinfo,ambinfo,dddataCurr); //if(nEpoch>1) spp.PassPreAmb(preambinfo,ambinfo,ddctrl.PhsTypeNo()); /* -----------------------------------------end cycle slip---------------------------------------- */ for (int s=0;s<dddataCurr.pairNum;s++) dddataCurr.tropCor[s]=ts.tropCor[s]; int obsNum =ddobsinfo.SumCod()+ddobsinfo.SumPhs();//the number of all obs in one system at current epoch int phsNum =ddobsinfo.SumPhs(); int ionoNum=dddataCurr.pairNum; int ambNum=ambinfo.SumUnfix(); //if(nEpoch>1)ambNum=preambinfo.SumUnfix(); if(ambNum==0) ambNum=1; /* equation part */ math::matrix<double> DesMatPos(obsNum,3); math::matrix<double> DesMatAmb(obsNum,ambNum); math::matrix<double> Weight(obsNum,obsNum); math::matrix<double> L(obsNum,1); math::matrix<double> DesMatTrop(obsNum,1); math::matrix<double> DesMatIono(obsNum,dddataCurr.pairNum); spp.FormDdErrEq(DesMatPos,DesMatTrop,DesMatIono,DesMatAmb,L,dddataCurr,ddctrl,ambinfo,ddobsinfo); math::matrix<double> DesMatTrop_Hour(obsNum,1); double firstTime=0.0,curTime=0.0; spp.FormDesMatTrop_ReNew(ddctrl, nEpoch, firstTime, curTime, roverData,dddataCurr, dddataPre,ptrMapCur, ptrMapPre,DesMatTrop, DesMatTrop_Hour,3600.0); Weight=spp.FormWeightVc(ddctrl,dddataCurr,ddobsinfo); math::matrix<double>DesAtmos=DesMatIono; DesMatAmb=ConvergeMat(DesMatTrop_Hour.RowNo(),DesMatTrop_Hour,DesMatAmb); math::matrix<double>Natmos=~DesMatIono*Weight*DesMatIono; math::matrix<double>Namb=~DesMatAmb*Weight*DesMatAmb; math::matrix<double>Natsam=~DesMatIono*Weight*DesMatAmb; math::matrix<double>Uatmos=~DesMatIono*Weight*L; math::matrix<double>Ua=~DesMatAmb*Weight*L; math::matrix<double> Uaaa,Uatat,Qaaa; math::matrix<double>Qamb,Qbb,Qb_amb; if (nEpoch==1) { Nneq=Namb; Uneq=Ua; Uaaa=Ua; Uatat=Uatmos; SolveNEQCholesky2(Natmos,Natsam,Nneq,Qamb,Qb_amb,Qbb,Uatat,Uaaa,Nneq.RowNo(),Natmos.RowNo()); /*reduction of atmos parameters*/ Nneq=Namb-~Natsam*CholeskyInv(Natmos)*Natsam; Uneq=Ua-~Natsam*CholeskyInv(Natmos)*Uatmos; } /****************************method 1**********************************Ё§*/ /*the LSE of current epoch*/ math::matrix<double>invNatmos=CholeskyInv(Natmos); math::matrix<double>Qaa=CholeskyInv( Namb-~Natsam*invNatmos*Natsam ); math::matrix<double>Qatsam=-invNatmos*Natsam*Qaa; math::matrix<double>Qats=CholeskyInv(Natmos-Natsam*CholeskyInv(Namb)*~Natsam); CurSolu[0]=Qats*Uatmos+Qatsam*Ua; CurSolu[1]=~Qatsam*Uatmos+Qaa*Ua; CurVcm[0]=Qats; CurVcm[1]=Qatsam; CurVcm[2]=Qaa; /****************************method 1******************* ***************Ёќ*/ if (nEpoch>1) { /*method 1: update the LSE solution with the previous ambiguity constraint*/ SuperPosLonBWithFixedCrd(ddctrl,PreVcmAmb,PreSoluAmb,CurVcm,CurSolu,preambinfo,ambinfo,dddataCurr,dddataPre); /*method 2: Normal equ*/ SuperPosLonBWithFixedCrdNEQ(ddctrl,Nneq,Uneq,Namb,Ua,preambinfo,ambinfo,dddataCurr,dddataPre); Uaaa= Uneq; Uatat=Uatmos; SolveNEQCholesky2(Natmos,Natsam,Nneq,Qamb,Qb_amb,Qbb,Uatat,Uaaa,Nneq.RowNo(),Natmos.RowNo()); /*now, Uatat is the solu of ionosphere, Uaaa is the solu of trop and amb*/ /*reduction of atmos parameters*/ Nneq-=~Natsam*CholeskyInv(Natmos)*Natsam; Uneq-=~Natsam*CholeskyInv(Natmos)*Uatmos; } /***************** try to fix the ambiguity******************/ math::matrix<double>a_hat=Uaaa,b_hat=Uatat; double ratio=0.0; //editVcm(Qamb,Qb_amb,Qbb,a_hat,b_hat); math::matrix<double>a_check; //a_check=FixAmbQXWZ(Qamb,Qb_amb,Qbb,a_hat,b_hat,2.0,ratio);/*fix all*/ math::matrix<double>a_hat_update,b_hat_update; double ratiothrs=1.5; double elemaskPar=35.0; DdAmbInfo ambinfo_temp=ambinfo; QXWZPartialARwithUpdate(Qamb,Qb_amb,Qbb,a_hat,b_hat,a_hat_update,a_check,b_hat_update, dddataCurr,ratio,elemaskPar,ratiothrs,ambinfo_temp); if (ratio>ratiothrs) { checkAR(ambinfo_temp,dddataCurr,ddctrl,ddobsinfo); //cout<<b_hat-GetBlockMat(b_hat_update,1,b_hat.RowNo(),1,1,2); ratio=ratio; } /*pass VCM and Solu*/ PreSoluAmb=CurSolu[1]; PreVcmAmb=CurVcm[2]; /*output the solu*/ finish=clock(); int t0,t1,t2,t3; t0=((int)roverData.sec%86400); t1=t0/3600; //hour t2=(t0-t1*3600)/60; //min t3=(t0-t1*3600-t2*60)%60;//sec cout<<t1<<" "<<t2<<" "<<t3<<" "<<(double)(finish-start)<<" ms "<<ratio<<endl; if (ratio<ratiothrs) { FileOutTropsIonoFloat(fout,Uatat,Uaaa,dddataCurr,ptrMapPre,1);//CurSolu[0],CurSolu[1] } else { FileOutTropsIonoFix(fout,b_hat_update,dddataCurr,ptrMapPre,1); } /*--------------------- end processing method------------------- */ /*pass the AmbInfo*/ preambinfo=ambinfo; dddataPre=dddataCurr; } else { if (dt>0.0) epochData2=readdata.GetEpochData(Ofile2,line2,obsHeader2,nEpoch2,eventFlag2); else epochData1=readdata.GetEpochData(Ofile1,line1,obsHeader1,nEpoch1,eventFlag1); } if (eventFlag1==10 || eventFlag2==10) nEpoch1++; }//end while fout.close(); Ofile1.Close(); Ofile2.Close(); delete[] broadephdata,gloeph; }
void ProcessSingle(CString commandFile,int sysidUse,const char* outFile) { ProcessRinex readdata; CString CommadFile =_T("CommandFile.txt"); //control block DdData dddataPre; DdData dddataCurr; SdData lastSdData; ObsEpochData roverData,baseData; //single system DdObsInfo ddobsinfo,preddobsinfo; DdAmbInfo ambinfo,preambinfo; int numCod=0,numPhs=0; math::matrix<double> prenorm[5]; double dt; SppCtrl sppctrl; Position spp; SppInfo sppinfo,baseInfo; SppInfoGlo sppinfoGlo,baseInfoGlo; DdCtrl ddctrl; InPutFileSet inputfile; double baseCrd[3],roverCrd[3]; DdCtrl ddCtrlPtr[MAXSYS]; ReadCommFile(ddCtrlPtr,inputfile,_T("CommandFile.txt"),baseCrd,roverCrd); inputfile.CheckPath(); ddctrl=ddCtrlPtr[sysidUse-1]; sppctrl.sysid=ddctrl.sysid; sppctrl.maskEle=ddctrl.maskele; //read broadcast eph CString NFileName=inputfile.fileEph[0]; BroadEphHeader brdephheader; BroadEphData* broadephdata=new BroadEphData[MAXEPHNUM]; BroadEphDataGlo* gloeph=new BroadEphDataGlo[MAXEPHNUM_GLO]; int nSate =0; int nSateGlo =0; readdata.ReadBroadEph(NFileName, brdephheader,broadephdata,gloeph, nSate,nSateGlo); //Rover station ObsHeader obsHeader1; ObsEpochData epochData1; CString OFileName1 =inputfile.fileRover[0];//E:\\Ъ§Он\\SHCORS\\066(0307)\\SHCH\\0001066J00.14O""0002066C00.14O"E:\\CUTB2014120124.obs CStdioFile Ofile1; Ofile1.Open(OFileName1,CFile::modeRead); CString line1; readdata.ReadObsHeader(Ofile1,line1,obsHeader1); //Base station for (int i=0;i<3;i++) baseInfo.recPos[i]=baseCrd[i]; ObsHeader obsHeader2; ObsEpochData epochData2; CString OFileName2 =inputfile.fileBase[0]; CStdioFile Ofile2; Ofile2.Open(OFileName2,CFile::modeRead); CString line2; readdata.ReadObsHeader(Ofile2,line2,obsHeader2); int nEpoch1=0,nEpoch2=0; int eventFlag1=0,eventFlag2=0; int nEpoch=0; //output to file fstream fout; fout.open(outFile,ios::out); while (eventFlag1!=10 && eventFlag2!=10) { epochData1.ZeroElem(); epochData2.ZeroElem(); epochData1=readdata.GetEpochData(Ofile1,line1,obsHeader1,nEpoch1,eventFlag1); epochData1.Sort(); epochData2=readdata.GetEpochData(Ofile2,line2,obsHeader2,nEpoch2,eventFlag2); epochData2.Sort(); if(epochData1.sateNum>=5&&ddctrl.CodTypeNo()>1) epochData1=epochData1.AutoShrink(2); if(epochData2.sateNum>=5&&ddctrl.CodTypeNo()>1) epochData2=epochData2.AutoShrink(2); dt=(epochData1.week-epochData2.week)*7*86400.0+(epochData1.sec-epochData2.sec); if (1)//mark fabs(dt)<0.5 { nEpoch++; lastSdData.ZeroElem(); roverData.ZeroElem(); baseData.ZeroElem(); sppinfo.ZeroElem(); baseInfo.ZeroElem(); if (sysidUse==1 || sysidUse==3 ||sysidUse==5) { spp.StandPosition(brdephheader,broadephdata,epochData1,nSate,sppctrl,sppinfo,roverData); spp.baseStn(baseInfo,broadephdata,epochData2,baseData,nSate,sppctrl); } else if(sysidUse==2) { spp.StandPosition(brdephheader,gloeph,epochData1,nSateGlo,sppctrl,sppinfoGlo,roverData);//Glo spp.baseStn(baseInfoGlo,gloeph,epochData2,baseData,nSateGlo,sppctrl);//glo } int t0,t1,t2,t3; t0=((int)roverData.sec%86400); t1=t0/3600; //hour t2=(t0-t1*3600)/60; //hour t3=(t0-t1*3600-t2*60)%60; #if FileOutSpp //fout<<"epoch: "<<t1<<" "<<t2<<" "<<t3<<endl; //SppFileOut(fout,sppinfo); #endif if (nEpoch>288) { nEpoch=nEpoch; } //continue; if(Norm(roverCrd,3)>0.0) PtrEqual(roverCrd,sppinfo.recPos,3); int refPrn=0; ddobsinfo.ZeroEle(); int tprn=0; refPrn=spp.SelectRefSate(baseInfo,sppinfo,5.0,baseData,roverData,lastSdData,ddobsinfo,0,tprn); int refpos; refpos=GetPos(lastSdData.prn,refPrn,lastSdData.satnum); dddataCurr.ZeroElem(); ambinfo.ZeroElem(); spp.DoubleDiff(refPrn,refpos,lastSdData,dddataCurr); if(dddataCurr.pairNum<4) exit(1); DdData ts=dddataCurr; /* -----------------------------------------end data preparation part ---------------------------------------*/ dddataCurr=spp.ComObsPhsCod(ddctrl,ddobsinfo,ambinfo,dddataCurr); if(nEpoch>1) spp.PassPreAmb(preambinfo,ambinfo,ddctrl.PhsTypeNo()); /* -----------------------------------------end cycle slip---------------------------------------- */ int obsNum =ddobsinfo.SumCod()+ddobsinfo.SumPhs();//the number of all obs in one system at current epoch int phsNum =ddobsinfo.SumPhs(); int ionoNum=dddataCurr.pairNum; int ambNum=ambinfo.SumUnfix(); //if(nEpoch>1)ambNum=preambinfo.SumUnfix(); if(ambNum==0) ambNum=1; /* equation part */ math::matrix<double> DesMatPos(obsNum,3); math::matrix<double> DesMatAmb(obsNum,ambNum); math::matrix<double> Weight(obsNum,obsNum); math::matrix<double> L(obsNum,1); math::matrix<double> DesMatTrop(obsNum,1); math::matrix<double> DesMatIono(obsNum,obsNum); spp.FormDdErrEq(DesMatPos,DesMatTrop,DesMatIono,DesMatAmb,L,dddataCurr,ddctrl,ambinfo,ddobsinfo); Weight=spp.FormWeightVc(ddctrl,dddataCurr,ddobsinfo); /*--------------------- end processing method------------------- */ int resultflag=0; double ratio=0.0; if(nEpoch%1000==0) cout<<nEpoch<<endl; if (nEpoch>0) { double xxb[3]; math::matrix<double>xb=SoluShortEpoch(ddctrl,DesMatPos,DesMatAmb,Weight,L,ddobsinfo,ambinfo,resultflag,ratio); preambinfo=ambinfo; for(int i=0;i<3;i++) xxb[i]=xb(i,0)+sppinfo.recPos[i]; fout<<resultflag<<" "<<dddataCurr.pairNum<<" "<<~XYZ2NEU(sppinfo.recPos,xxb); //cout<<~XYZ2NEU(sppinfo.recPos,xxb); preddobsinfo=ddobsinfo; } // int t0,t1,t2,t3; //t0=((int)dddataCurr.sec%86400); //t1=t0/3600; //hour //t2=(t0-t1*3600)/60; //hour //t3=(t0-t1*3600-t2*60)%60; //cout<<"ratio "<<ratio<<" "<<t1<<" "<<t2<<" "<<t3<<" "<<dddataCurr.pairNum<<" "<<nEpoch<<endl; //cout<<endl; dddataPre =dddataCurr; /* end equation part */ } else { if (dt>0.0) epochData2=readdata.GetEpochData(Ofile2,line2,obsHeader2,nEpoch2,eventFlag2); else epochData1=readdata.GetEpochData(Ofile1,line1,obsHeader1,nEpoch1,eventFlag1); } if (eventFlag1==10 || eventFlag2==10) nEpoch1++; }//end while fout.close(); Ofile1.Close(); Ofile2.Close(); delete[] broadephdata,gloeph; }