Index: trunk/BNC/src/PPP_SSR_I/pppFilter.cpp =================================================================== --- trunk/BNC/src/PPP_SSR_I/pppFilter.cpp (revision 7286) +++ trunk/BNC/src/PPP_SSR_I/pppFilter.cpp (revision 7287) @@ -35,5 +35,5 @@ * Created: 01-Dec-2009 * - * Changes: + * Changes: * * -----------------------------------------------------------------------*/ @@ -66,5 +66,5 @@ // Constructor //////////////////////////////////////////////////////////////////////////// -t_pppParam::t_pppParam(t_pppParam::parType typeIn, int indexIn, +t_pppParam::t_pppParam(t_pppParam::parType typeIn, int indexIn, const QString& prnIn) { type = typeIn; @@ -90,11 +90,11 @@ // ----------- if (type == CRD_X) { - return (xx - satData->xx(1)) / satData->rho; + return (xx - satData->xx(1)) / satData->rho; } else if (type == CRD_Y) { - return (xx - satData->xx(2)) / satData->rho; + return (xx - satData->xx(2)) / satData->rho; } else if (type == CRD_Z) { - return (xx - satData->xx(3)) / satData->rho; + return (xx - satData->xx(3)) / satData->rho; } @@ -108,5 +108,5 @@ // ----------- else if (type == TROPO) { - return 1.0 / sin(satData->eleSat); + return 1.0 / sin(satData->eleSat); } @@ -238,5 +238,5 @@ } - _QQ.ReSize(_params.size()); + _QQ.ReSize(_params.size()); _QQ = 0.0; for (int iPar = 1; iPar <= _params.size(); iPar++) { @@ -244,11 +244,11 @@ pp->xx = 0.0; if (pp->isCrd()) { - _QQ(iPar,iPar) = OPT->_aprSigCrd(1) * OPT->_aprSigCrd(1); + _QQ(iPar,iPar) = OPT->_aprSigCrd(1) * OPT->_aprSigCrd(1); } else if (pp->type == t_pppParam::RECCLK) { - _QQ(iPar,iPar) = OPT->_noiseClk * OPT->_noiseClk; + _QQ(iPar,iPar) = OPT->_noiseClk * OPT->_noiseClk; } else if (pp->type == t_pppParam::TROPO) { - _QQ(iPar,iPar) = OPT->_aprSigTrp * OPT->_aprSigTrp; + _QQ(iPar,iPar) = OPT->_aprSigTrp * OPT->_aprSigTrp; pp->xx = lastTrp; } @@ -327,8 +327,8 @@ double rho0 = (satData->xx - xRec).norm_Frobenius(); - double dPhi = t_CST::omega * rho0 / t_CST::c; - - xRec(1) = x() * cos(dPhi) - y() * sin(dPhi); - xRec(2) = y() * cos(dPhi) + x() * sin(dPhi); + double dPhi = t_CST::omega * rho0 / t_CST::c; + + xRec(1) = x() * cos(dPhi) - y() * sin(dPhi); + xRec(2) = y() * cos(dPhi) + x() * sin(dPhi); xRec(3) = z(); @@ -337,5 +337,5 @@ satData->rho = (satData->xx - xRec).norm_Frobenius(); - double tropDelay = delay_saast(satData->eleSat) + + double tropDelay = delay_saast(satData->eleSat) + trp() / sin(satData->eleSat); @@ -364,5 +364,5 @@ } double phaseCenter = 0.0; - if (_antex) { + if (_antex) { bool found; phaseCenter = satData->lkA * _antex->rcvCorr(OPT->_antNameRover, frqA, @@ -382,9 +382,9 @@ double cose = cos(satData->eleSat); double sine = sin(satData->eleSat); - antennaOffset = -OPT->_neuEccRover(1) * cosa*cose - -OPT->_neuEccRover(2) * sina*cose + antennaOffset = -OPT->_neuEccRover(1) * cosa*cose + -OPT->_neuEccRover(2) * sina*cose -OPT->_neuEccRover(3) * sine; - return satData->rho + phaseCenter + antennaOffset + clk() + return satData->rho + phaseCenter + antennaOffset + clk() + offset - satData->clk + tropDelay + wind; } @@ -396,9 +396,9 @@ Tracer tracer("t_pppFilter::delay_saast"); - double xyz[3]; + double xyz[3]; xyz[0] = x(); xyz[1] = y(); xyz[2] = z(); - double ell[3]; + double ell[3]; xyz2ell(xyz, ell); double height = ell[2]; @@ -410,11 +410,11 @@ double h_km = height / 1000.0; - + if (h_km < 0.0) h_km = 0.0; if (h_km > 5.0) h_km = 5.0; int ii = int(h_km + 1); double href = ii - 1; - - double bCor[6]; + + double bCor[6]; bCor[0] = 1.156; bCor[1] = 1.006; @@ -423,7 +423,7 @@ bCor[4] = 0.654; bCor[5] = 0.563; - + double BB = bCor[ii-1] + (bCor[ii]-bCor[ii-1]) * (h_km - href); - + double zen = M_PI/2.0 - Ele; @@ -447,5 +447,5 @@ reset(); } - + // Use different white noise for Quick-Start mode // ---------------------------------------------- @@ -459,5 +459,5 @@ for (int iPar = 1; iPar <= _params.size(); iPar++) { t_pppParam* pp = _params[iPar-1]; - + // Coordinates // ----------- @@ -494,6 +494,6 @@ } _QQ(iPar,iPar) += sigCrdP_used * sigCrdP_used; - } - + } + // Receiver Clocks // --------------- @@ -505,5 +505,5 @@ _QQ(iPar,iPar) = OPT->_noiseClk * OPT->_noiseClk; } - + // Tropospheric Delay // ------------------ @@ -511,5 +511,5 @@ _QQ(iPar,iPar) += OPT->_noiseTrp * OPT->_noiseTrp; } - + // Glonass Offset // -------------- @@ -531,5 +531,5 @@ // ---------- else if (pp->type == t_pppParam::BDS_OFFSET) { - _QQ(iPar,iPar) = 1000.0 * 1000.0; //TODO: TEST + _QQ(iPar,iPar) += 0.1 * 0.1; //TODO: TEST } } @@ -544,10 +544,10 @@ // ----------------------------------------------- SymmetricMatrix QQ_old = _QQ; - + for (int iPar = 1; iPar <= _params.size(); iPar++) { _params[iPar-1]->index_old = _params[iPar-1]->index; _params[iPar-1]->index = 0; } - + // Remove Ambiguity Parameters without observations // ------------------------------------------------ @@ -569,5 +569,5 @@ } } - + // Add new ambiguity parameters // ---------------------------- @@ -578,5 +578,5 @@ addAmb(satData); } - + int nPar = _params.size(); _QQ.ReSize(nPar); _QQ = 0.0; @@ -593,5 +593,5 @@ } } - + for (int ii = 1; ii <= nPar; ii++) { t_pppParam* par = _params[ii-1]; @@ -634,6 +634,6 @@ t_pppParam* par = itPar.next(); if (par->type == t_pppParam::RECCLK) { - LOG << "\n clk = " << setw(10) << setprecision(3) << par->xx - << " +- " << setw(6) << setprecision(3) + LOG << "\n clk = " << setw(10) << setprecision(3) << par->xx + << " +- " << setw(6) << setprecision(3) << sqrt(_QQ(par->index,par->index)); } @@ -641,6 +641,6 @@ ++par->numEpo; LOG << "\n amb " << par->prn.mid(0,3).toAscii().data() << " = " - << setw(10) << setprecision(3) << par->xx - << " +- " << setw(6) << setprecision(3) + << setw(10) << setprecision(3) << par->xx + << " +- " << setw(6) << setprecision(3) << sqrt(_QQ(par->index,par->index)) << " nEpo = " << par->numEpo; @@ -651,15 +651,15 @@ << setw(7) << setprecision(3) << aprTrp << " " << setw(6) << setprecision(3) << showpos << par->xx << noshowpos - << " +- " << setw(6) << setprecision(3) + << " +- " << setw(6) << setprecision(3) << sqrt(_QQ(par->index,par->index)); } else if (par->type == t_pppParam::GLONASS_OFFSET) { - LOG << "\n offGlo = " << setw(10) << setprecision(3) << par->xx - << " +- " << setw(6) << setprecision(3) + LOG << "\n offGlo = " << setw(10) << setprecision(3) << par->xx + << " +- " << setw(6) << setprecision(3) << sqrt(_QQ(par->index,par->index)); } else if (par->type == t_pppParam::GALILEO_OFFSET) { - LOG << "\n offGal = " << setw(10) << setprecision(3) << par->xx - << " +- " << setw(6) << setprecision(3) + LOG << "\n offGal = " << setw(10) << setprecision(3) << par->xx + << " +- " << setw(6) << setprecision(3) << sqrt(_QQ(par->index,par->index)); } @@ -679,5 +679,5 @@ // Final Message (both log file and screen) // ---------------------------------------- - LOG << OPT->_roverName << " PPP " + LOG << OPT->_roverName << " PPP " << epoData->tt.timestr(1) << " " << epoData->sizeAll() << " " << setw(14) << setprecision(3) << x() << " +- " @@ -722,14 +722,14 @@ QString prnGPS; QString prnGlo; - double maxResGPS = 0.0; // all the other systems except GLONASS - double maxResGlo = 0.0; // GLONASS + double maxResGPS = 0.0; // GPS + Galileo + double maxResGlo = 0.0; // GLONASS + BDS findMaxRes(vv, satData, prnGPS, prnGlo, maxResGPS, maxResGlo); if (iPhase == 1) { - if (maxResGlo > 2.98 * OPT->_maxResL1) { + if (maxResGlo > 2.98 * OPT->_maxResL1) { LOG << "Outlier Phase " << prnGlo.mid(0,3).toAscii().data() << ' ' << maxResGlo << endl; return prnGlo; } - else if (maxResGPS > 2.98 * OPT->_maxResL1) { + else if (maxResGPS > 2.98 * OPT->_maxResL1) { LOG << "Outlier Phase " << prnGPS.mid(0,3).toAscii().data() << ' ' << maxResGPS << endl; return prnGPS; @@ -762,5 +762,5 @@ // ----------------------------------- if (!_windUpTime.contains(prn) || _windUpTime[prn] != Mjd) { - _windUpTime[prn] = Mjd; + _windUpTime[prn] = Mjd; // Unit Vector GPS Satellite --> Receiver @@ -768,5 +768,5 @@ ColumnVector rho = rRec - rSat; rho /= rho.norm_Frobenius(); - + // GPS Satellite unit Vectors sz, sy, sx // ------------------------------------- @@ -781,7 +781,7 @@ // Effective Dipole of the GPS Satellite Antenna // --------------------------------------------- - ColumnVector dipSat = sx - rho * DotProduct(rho,sx) + ColumnVector dipSat = sx - rho * DotProduct(rho,sx) - crossproduct(rho, sy); - + // Receiver unit Vectors rx, ry // ---------------------------- @@ -791,30 +791,30 @@ double recEll[3]; xyz2ell(rRec.data(), recEll) ; double neu[3]; - + neu[0] = 1.0; neu[1] = 0.0; neu[2] = 0.0; neu2xyz(recEll, neu, rx.data()); - + neu[0] = 0.0; neu[1] = -1.0; neu[2] = 0.0; neu2xyz(recEll, neu, ry.data()); - + // Effective Dipole of the Receiver Antenna // ---------------------------------------- - ColumnVector dipRec = rx - rho * DotProduct(rho,rx) + ColumnVector dipRec = rx - rho * DotProduct(rho,rx) + crossproduct(rho, ry); - + // Resulting Effect // ---------------- - double alpha = DotProduct(dipSat,dipRec) / + double alpha = DotProduct(dipSat,dipRec) / (dipSat.norm_Frobenius() * dipRec.norm_Frobenius()); - + if (alpha > 1.0) alpha = 1.0; if (alpha < -1.0) alpha = -1.0; - + double dphi = acos(alpha) / 2.0 / M_PI; // in cycles - + if ( DotProduct(rho, crossproduct(dipSat, dipRec)) < 0.0 ) { dphi = -dphi; @@ -824,8 +824,8 @@ } - return _windUpSum[prn]; -} - -// + return _windUpSum[prn]; +} + +// /////////////////////////////////////////////////////////////////////////// void t_pppFilter::cmpEle(t_satData* satData) { @@ -844,5 +844,5 @@ } -// +// /////////////////////////////////////////////////////////////////////////// void t_pppFilter::addAmb(t_satData* satData) { @@ -853,5 +853,5 @@ bool found = false; for (int iPar = 1; iPar <= _params.size(); iPar++) { - if (_params[iPar-1]->type == t_pppParam::AMB_L3 && + if (_params[iPar-1]->type == t_pppParam::AMB_L3 && _params[iPar-1]->prn == satData->prn) { found = true; @@ -860,5 +860,5 @@ } if (!found) { - t_pppParam* par = new t_pppParam(t_pppParam::AMB_L3, + t_pppParam* par = new t_pppParam(t_pppParam::AMB_L3, _params.size()+1, satData->prn); _params.push_back(par); @@ -867,5 +867,5 @@ } -// +// /////////////////////////////////////////////////////////////////////////// void t_pppFilter::addObs(int iPhase, unsigned& iObs, t_satData* satData, @@ -876,5 +876,5 @@ const double ELEWGHT = 20.0; double ellWgtCoef = 1.0; - double eleD = satData->eleSat * 180.0 / M_PI; + double eleD = satData->eleSat * 180.0 / M_PI; if (eleD < ELEWGHT) { ellWgtCoef = 1.5 - 0.5 / (ELEWGHT - 10.0) * (eleD - 10.0); @@ -903,5 +903,5 @@ _params[iPar-1]->prn == satData->prn) { ll(iObs) -= _params[iPar-1]->xx; - } + } AA(iObs, iPar) = _params[iPar-1]->partial(satData, true); } @@ -912,7 +912,4 @@ else { double sigP3 = 2.98 * OPT->_sigmaC1; - if (satData->system() == 'C') { - sigP3 *= BDS_WEIGHT_FACTOR; - } ll(iObs) = satData->P3 - cmpValue(satData, false); PP(iObs,iObs) = 1.0 / (sigP3 * sigP3) / (ellWgtCoef * ellWgtCoef); @@ -923,7 +920,7 @@ } -// +// /////////////////////////////////////////////////////////////////////////// -QByteArray t_pppFilter::printRes(int iPhase, const ColumnVector& vv, +QByteArray t_pppFilter::printRes(int iPhase, const ColumnVector& vv, const QMap& satDataMap) { @@ -950,10 +947,10 @@ } -// +// /////////////////////////////////////////////////////////////////////////// void t_pppFilter::findMaxRes(const ColumnVector& vv, const QMap& satData, - QString& prnGPS, QString& prnGlo, - double& maxResGPS, double& maxResGlo) { + QString& prnGPS, QString& prnGlo, + double& maxResGPS, double& maxResGlo) { Tracer tracer("t_pppFilter::findMaxRes"); @@ -968,5 +965,5 @@ if (satData->obsIndex != 0) { QString prn = satData->prn; - if (prn[0] == 'R') { + if (prn[0] == 'R' || prn[0] == 'C') { if (fabs(vv(satData->obsIndex)) > maxResGlo) { maxResGlo = fabs(vv(satData->obsIndex)); @@ -983,5 +980,5 @@ } } - + // Update Step (private - loop over outliers) //////////////////////////////////////////////////////////////////////////// @@ -1010,5 +1007,5 @@ // First update using code observations, then phase observations - // ------------------------------------------------------------- + // ------------------------------------------------------------- bool usePhase = OPT->ambLCs('G').size() || OPT->ambLCs('R').size() || OPT->ambLCs('E').size() || OPT->ambLCs('C').size() ; @@ -1019,5 +1016,5 @@ // ----------------- predict(iPhase, epoData); - + // Create First-Design Matrix // -------------------------- @@ -1034,11 +1031,11 @@ } } - + // Prepare first-design Matrix, vector observed-computed // ----------------------------------------------------- Matrix AA(nObs, nPar); // first design matrix - ColumnVector ll(nObs); // tems observed-computed + ColumnVector ll(nObs); // terms observed-computed DiagonalMatrix PP(nObs); PP = 0.0; - + unsigned iObs = 0; QMapIterator it(epoData->satData); @@ -1061,8 +1058,8 @@ kalman(AA, ll, PP, _QQ, dx); ColumnVector vv = ll - AA * dx; - + // Print Residuals // --------------- - if (iPhase == 0) { + if (iPhase == 0) { strResCode = printRes(iPhase, vv, epoData->satData); } @@ -1162,10 +1159,10 @@ } -// -//////////////////////////////////////////////////////////////////////////// -t_irc t_pppFilter::selectSatellites(const QString& lastOutlierPrn, +// +//////////////////////////////////////////////////////////////////////////// +t_irc t_pppFilter::selectSatellites(const QString& lastOutlierPrn, QMap& satData) { - // First Call + // First Call // ---------- if (lastOutlierPrn.isEmpty()) { @@ -1179,5 +1176,5 @@ else { - if (lastOutlierPrn[0] == 'R') { + if (lastOutlierPrn[0] == 'R' || lastOutlierPrn[0] == 'C') { _outlierGlo << lastOutlierPrn; } @@ -1193,5 +1190,5 @@ } - if (lastOutlierPrn[0] == 'R') { + if (lastOutlierPrn[0] == 'R' || lastOutlierPrn[0] == 'C') { _outlierGPS.clear(); return success; @@ -1213,5 +1210,5 @@ } -// +// //////////////////////////////////////////////////////////////////////////// double lorentz(const ColumnVector& aa, const ColumnVector& bb) { @@ -1219,5 +1216,5 @@ } -// +// //////////////////////////////////////////////////////////////////////////// void t_pppFilter::bancroft(const Matrix& BBpass, ColumnVector& pos) { @@ -1237,6 +1234,6 @@ if (iter > 1) { double zz = BB(ii,3); - double rho = sqrt( (xx-pos(1)) * (xx-pos(1)) + - (yy-pos(2)) * (yy-pos(2)) + + double rho = sqrt( (xx-pos(1)) * (xx-pos(1)) + + (yy-pos(2)) * (yy-pos(2)) + (zz-pos(3)) * (zz-pos(3)) ); traveltime = rho / t_CST::c; @@ -1248,5 +1245,5 @@ BB(ii,2) = -sina * xx + cosa * yy; } - + Matrix BBB; if (mm > 4) { @@ -1260,5 +1257,5 @@ ColumnVector alpha(mm); alpha = 0.0; for (int ii = 1; ii <= mm; ii++) { - alpha(ii) = lorentz(BB.Row(ii).t(),BB.Row(ii).t())/2.0; + alpha(ii) = lorentz(BB.Row(ii).t(),BB.Row(ii).t())/2.0; } ColumnVector BBBe = BBB * ee; @@ -1269,5 +1266,5 @@ double root = sqrt(bb*bb-aa*cc); - Matrix hlpPos(4,2); + Matrix hlpPos(4,2); hlpPos.Column(1) = (-bb-root)/aa * BBBe + BBBalpha; hlpPos.Column(2) = (-bb+root)/aa * BBBe + BBBalpha; @@ -1276,8 +1273,8 @@ for (int pp = 1; pp <= 2; pp++) { hlpPos(4,pp) = -hlpPos(4,pp); - omc(pp) = BB(1,4) - + omc(pp) = BB(1,4) - sqrt( (BB(1,1)-hlpPos(1,pp)) * (BB(1,1)-hlpPos(1,pp)) + (BB(1,2)-hlpPos(2,pp)) * (BB(1,2)-hlpPos(2,pp)) + - (BB(1,3)-hlpPos(3,pp)) * (BB(1,3)-hlpPos(3,pp)) ) - + (BB(1,3)-hlpPos(3,pp)) * (BB(1,3)-hlpPos(3,pp)) ) - hlpPos(4,pp); } @@ -1291,5 +1288,5 @@ } -// +// //////////////////////////////////////////////////////////////////////////// void t_pppFilter::cmpDOP(t_epoData* epoData) { @@ -1319,7 +1316,7 @@ } AA = AA.Rows(1, _numSat); - SymmetricMatrix NN; NN << AA.t() * AA; + SymmetricMatrix NN; NN << AA.t() * AA; SymmetricMatrix QQ = NN.i(); - + _pDop = sqrt(QQ(1,1) + QQ(2,2) + QQ(3,3)); }