Changeset 8956 in ntrip for trunk/BNC/src/PPP/pppFilter.cpp
- Timestamp:
- Jun 23, 2020, 11:58:46 AM (4 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/BNC/src/PPP/pppFilter.cpp
r8915 r8956 83 83 // Set Parameters 84 84 // -------------- 85 _parlist->set(_epoTime, allObs );85 _parlist->set(_epoTime, allObs, _obsPool->getRefSatMap()); 86 86 const vector<t_pppParam*>& params = _parlist->params(); 87 87 … … 90 90 ColumnVector xFltOld = _xFlt; 91 91 SymmetricMatrix QFltOld = _QFlt; 92 92 93 _QFlt.ReSize(_parlist->nPar()); _QFlt = 0.0; 93 94 _xFlt.ReSize(_parlist->nPar()); _xFlt = 0.0; … … 114 115 predictCovCrdPart(QFltOld); 115 116 117 // Init Datum Trafo 118 // ---------------------------------------- 119 if ((OPT->_obsModelType == OPT->DCMcodeBias || 120 OPT->_obsModelType == OPT->DCMphaseBias) && 121 (_numEpoProcessing == 1)) { 122 _numAllUsedLCs = 0; 123 for (unsigned iSys = 0; iSys < OPT->systems().size(); iSys++) { 124 char system = OPT->systems()[iSys]; 125 _numAllUsedLCs += OPT->LCs(system).size(); 126 if (OPT->_pseudoObsIono && !epoch->pseudoObsIono()) { 127 _numAllUsedLCs -= 1; // GIM not used 128 } 129 } 130 int maxObs = allObs.size() * _numAllUsedLCs; 131 _datumTrafo->initAA(maxObs, _parlist->nPar()); 132 } 116 133 117 134 // Pre-Process Satellite Systems separately … … 121 138 OPT->_obsModelType == OPT->DCMphaseBias) { 122 139 preProcessing = true; 123 _numAllUsedLCs = 0;124 140 for (unsigned iSys = 0; iSys < OPT->systems().size(); iSys++) { 141 (iSys) ? _datumTrafo->setFirstSystem(false) : _datumTrafo->setFirstSystem(true); 125 142 char system = OPT->systems()[iSys]; 126 _numAllUsedLCs += OPT->LCs(system).size();127 if (OPT->_pseudoObsIono && !epoch->pseudoObsIono()) {128 _numAllUsedLCs -= 1; // GIM not used129 }130 143 if (OPT->_refSatRequired) { 131 144 _refPrn = (_obsPool->getRefSatMapElement(system))->prn(); 145 if (_obsPool->hasHistoricalRefSat(_refPrn)) { 146 return failure; 147 } 132 148 } 133 149 vector<t_pppSatObs*> obsVector; … … 142 158 } 143 159 } 144 } 145 146 if (_numEpoProcessing == 1) { 147 int maxObs = allObs.size() * _numAllUsedLCs; 148 _datumTrafo->initAA(maxObs, _parlist->nPar()); 160 // refSat change required? 161 // ----------------------- 162 if (_obsPool->refSatChangeRequired()) { 163 _xFlt = xFltOld; 164 _QFlt = QFltOld; 165 return success; 166 } 167 else if (!_obsPool->refSatChangeRequired() && 168 _numEpoProcessing > 1) { 169 datumTransformation(xFltOld, QFltOld); 170 } 149 171 } 150 172 … … 154 176 for (unsigned iSys = 0; iSys < OPT->systems().size(); iSys++) { 155 177 char system = OPT->systems()[iSys]; 156 (iSys) ? _datumTrafo->setFirstSystem(false) :157 _datumTrafo->setFirstSystem(true);158 178 if (OPT->_refSatRequired) { 159 179 _refPrn = (_obsPool->getRefSatMapElement(system))->prn(); … … 174 194 } 175 195 176 // refSat change required?177 // -----------------------178 if (_obsPool->refSatChangeRequired()) {179 _xFlt = xFltOld;180 _QFlt = QFltOld;181 }182 196 // close epoch processing 183 197 // ---------------------- 184 else { 185 cmpDOP(allObs); 186 _parlist->printResult(_epoTime, _QFlt, _xFlt); 187 _lastEpoTimeOK = _epoTime; // remember time of last successful epoch processing 188 } 198 cmpDOP(allObs); 199 _parlist->printResult(_epoTime, _QFlt, _xFlt); 200 _lastEpoTimeOK = _epoTime; // remember time of last successful epoch processing 189 201 190 202 return success; … … 198 210 bool pseudoObsIonoAvailable, 199 211 bool preProcessing) { 200 qDebug() << "======t_pppFilter::processSystem=======";201 212 LOG.setf(ios::fixed); 202 213 203 214 // Detect Cycle Slips 204 215 // ------------------ 216 205 217 if (detectCycleSlips(LCs, obsVector, refPrn, preProcessing) != success) { 206 218 return failure; … … 221 233 maxObs -= 1; 222 234 } 223 qDebug() << "par.size() : " << _parlist->nPar() << " LCs.size(): " << usedLCs;224 qDebug() << "obsVector.size(): " << obsVector.size() << " maxObs : " << maxObs;225 235 226 236 // Outlier Detection Loop 227 237 // ---------------------- 228 238 for (unsigned iOutlier = 0; iOutlier < maxObs; iOutlier++) { 229 qDebug() << "iOutlier: " << iOutlier;230 239 231 240 if (iOutlier > 0) { … … 236 245 // First-Design Matrix, Terms Observed-Computed, Weight Matrix 237 246 // ----------------------------------------------------------- 238 qDebug() << "A(" << maxObs << "," << _parlist->nPar() << ")";239 247 Matrix AA(maxObs, _parlist->nPar()); 240 248 ColumnVector ll(maxObs); 241 249 DiagonalMatrix PP(maxObs); PP = 0.0; 242 243 // TETSPLOT244 for (unsigned iPar = 0; iPar < params.size(); iPar++) {245 const t_pppParam* par = params[iPar];246 cout << " " << par->toString() <<endl;247 }248 cout << endl;249 //END TETSPLOT250 250 251 251 int iObs = -1; … … 253 253 vector<t_lc::type> usedTypes; 254 254 for (unsigned ii = 0; ii < obsVector.size(); ii++) { 255 t_pppSatObs* obs = obsVector[ii]; qDebug() << "SATELLITE: " << obs->prn().toString().c_str() << "isRef: " << obs->isReference();255 t_pppSatObs* obs = obsVector[ii]; 256 256 if (!obs->outlier()) { 257 257 for (unsigned jj = 0; jj < usedLCs; jj++) { … … 263 263 for (unsigned iPar = 0; iPar < params.size(); iPar++) { 264 264 const t_pppParam* par = params[iPar]; 265 AA[iObs][iPar] = par->partial(_epoTime, obs, tLC); 266 cout << setw(10) << par->partial(_epoTime, obs, tLC); 267 } 268 cout << endl; 265 AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn); 266 } 269 267 ll[iObs] = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA.Row(iObs+1)); 270 268 PP[iObs] = 1.0 / (obs->sigma(tLC) * obs->sigma(tLC)); 271 //qDebug() << "ll[iObs]: " << ll[iObs]; 272 //qDebug() << "PP[iObs]: " << PP[iObs]; 273 } 274 } 275 } 276 277 if ((!preProcessing) && 278 (OPT->_obsModelType == OPT->DCMcodeBias || 279 OPT->_obsModelType == OPT->DCMphaseBias)) { 269 } 270 } 271 } 272 273 if ((preProcessing && ! iOutlier) && 274 (OPT->_obsModelType == OPT->DCMcodeBias || OPT->_obsModelType == OPT->DCMphaseBias)) { 280 275 _datumTrafo->updateIndices(maxObs); 281 276 _datumTrafo->prepareAA(AA, _numEpoProcessing, _parlist->nPar()); 277 if (_obsPool->refSatChangeRequired()) { // from detectCycleSlips() 278 return success; 279 } 282 280 } 283 281 … … 331 329 } 332 330 } 333 if (par &&preProcessing) {334 if (par ->prn() == refPrn) {331 if (preProcessing) { 332 if (par && obs->prn() == refPrn) { 335 333 _obsPool->setRefSatChangeRequired(true); 336 } 337 } 338 else if (par && !preProcessing) { 339 if (par->ambResetCandidate()) { 340 resetAmb(par->prn(), obsVector, &QSav, &xSav); 334 break; 335 } 336 } 337 else { 338 if (par) { 339 if (par->ambResetCandidate() || _obsPool->hasHistoricalRefSat(par->prn())) { 340 resetAmb(par->prn(), obsVector, &QSav, &xSav); 341 } 342 else { 343 par->setAmbResetCandidate(); 344 obs->setOutlier(); 345 } 341 346 } 342 347 else { 343 par->setAmbResetCandidate();344 348 obs->setOutlier(); 345 349 } 346 350 } 347 else if (!par && !preProcessing){ 348 obs->setOutlier(); 349 } 350 } 351 351 } 352 352 // Print Residuals 353 353 // --------------- … … 381 381 const t_prn& refPrn, 382 382 bool preProcessing) { 383 384 const double SLIP = 20.0; // slip threshold 383 if (_obsPool->hasHistoricalRefSat(refPrn)) { 384 return success; 385 } 386 387 double SLIP = 20.0; // slip threshold 385 388 string epoTimeStr = string(_epoTime); 386 389 const vector<t_pppParam*>& params = _parlist->params(); 390 391 if (preProcessing) { 392 SLIP += 10.0; 393 } 387 394 388 395 for (unsigned ii = 0; ii < LCs.size(); ii++) { … … 391 398 for (unsigned iObs = 0; iObs < obsVector.size(); iObs++) { 392 399 const t_pppSatObs* obs = obsVector[iObs]; 400 401 if (preProcessing && obs->prn() != refPrn) {continue;} 393 402 394 403 // Check set Slips and Jump Counters … … 405 414 slip = true; 406 415 } 407 _slips[obs->prn()]._obsSlipCounter = obs->slipCounter(); 416 if (!preProcessing) { 417 _slips[obs->prn()]._obsSlipCounter = obs->slipCounter(); 418 } 408 419 409 420 if (_slips[obs->prn()]._biasJumpCounter != -1 && … … 412 423 slip = true; 413 424 } 414 425 if (!preProcessing) { 426 _slips[obs->prn()]._biasJumpCounter = obs->biasJumpCounter(); 427 } 415 428 // Slip Set 416 429 // -------- 417 430 if (slip) { 418 431 if (preProcessing) { 419 if (obs->prn() == refPrn) { 420 _obsPool->setRefSatChangeRequired(true); 421 } 432 _obsPool->setRefSatChangeRequired(true); 422 433 } 423 434 else { … … 431 442 for (unsigned iPar = 0; iPar < params.size(); iPar++) { 432 443 const t_pppParam* par = params[iPar]; 433 AA[iPar] = par->partial(_epoTime, obs, tLC); 434 } 444 AA[iPar] = par->partial(_epoTime, obs, tLC, refPrn); 445 } 446 435 447 double ll = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA); 436 448 double vv = DotProduct(AA, _xFlt) - ll; 437 449 if (fabs(vv) > SLIP) { 438 450 if (preProcessing) { 439 if (obs->prn() == refPrn) { 440 _obsPool->setRefSatChangeRequired(true); 441 } 451 _obsPool->setRefSatChangeRequired(true); 442 452 } 443 453 else { … … 451 461 } 452 462 } 453 454 463 return success; 455 464 } … … 459 468 t_irc t_pppFilter::resetAmb(t_prn prn, const vector<t_pppSatObs*>& obsVector, 460 469 SymmetricMatrix* QSav, ColumnVector* xSav) { 470 461 471 t_irc irc = failure; 462 472 vector<t_pppParam*>& params = _parlist->params(); … … 504 514 for (unsigned ii = 0; ii < obsVector.size(); ii++) { 505 515 t_pppSatObs* obs = obsVector[ii]; 516 char system = obs->prn().system(); 517 t_prn refPrn = t_prn(); 518 if (OPT->_refSatRequired) { 519 refPrn = _obsPool->getRefSatMapElement(system)->prn(); 520 } 506 521 if (obs->isValid() && !obs->outlier()) { 507 522 ++_numSat; 508 523 for (unsigned iPar = 0; iPar < numPar; iPar++) { 509 524 const t_pppParam* par = _parlist->params()[iPar]; 510 AA[_numSat-1][iPar] = par->partial(_epoTime, obs, t_lc::c1 );525 AA[_numSat-1][iPar] = par->partial(_epoTime, obs, t_lc::c1, refPrn); 511 526 } 512 527 } … … 571 586 // Compute datum transformation 572 587 //////////////////////////////////////////////////////////////////////////// 573 void t_pppFilter::datumTransformation(void) { 574 _QFlt = _datumTrafo->varCov(Q()); 575 } 576 588 void t_pppFilter::datumTransformation(const ColumnVector& xFltOld, const SymmetricMatrix& QFltOld) { 589 590 Matrix D21 = _datumTrafo->computeTrafoMatrix(); 591 _QFlt << D21 * QFltOld * D21.t(); 592 _xFlt = D21 * xFltOld; 593 } 594 595 596 // Reset Ambiguity Parameter (cycle slip) 597 //////////////////////////////////////////////////////////////////////////// 598 t_irc t_pppFilter::addInfiniteNoise(t_pppParam::e_type para) { 599 t_irc irc = failure; 600 vector<t_pppParam*>& params = _parlist->params(); 601 for (unsigned iPar = 0; iPar < params.size(); iPar++) { 602 t_pppParam* par = params[iPar]; 603 if (par->type() == para) { 604 int ind = par->indexNew(); 605 if (ind < 0) { 606 return irc; 607 } 608 _QFlt(ind+1,ind+1) += par->sigma0() * par->sigma0(); 609 irc = success; 610 } 611 } 612 return irc; 613 }
Note:
See TracChangeset
for help on using the changeset viewer.