Changeset 10034 in ntrip for trunk/BNC/src/PPP/pppFilter.cpp


Ignore:
Timestamp:
Apr 21, 2023, 11:48:24 AM (12 months ago)
Author:
stuerze
Message:
 
File:
1 edited

Legend:

Unmodified
Added
Removed

  • trunk/BNC/src/PPP/pppFilter.cpp

    r10033 r10034  
    2727#include "pppObsPool.h"
    2828#include "pppStation.h"
     29#include "pppClient.h"
    2930
    3031using namespace BNC_PPP;
    … …  
    3334// Constructor
    3435////////////////////////////////////////////////////////////////////////////
    35 t_pppFilter::t_pppFilter(t_pppObsPool *obsPool) {
    36   _numSat = 0;
    37   _obsPool = obsPool;
    38   _refPrn = t_prn();
    39   _datumTrafo = new t_datumTrafo();
     36t_pppFilter::t_pppFilter() {
     37  _parlist = 0;
    4038}
    4139
    … …  
    4341////////////////////////////////////////////////////////////////////////////
    4442t_pppFilter::~t_pppFilter() {
    45   delete _datumTrafo;
     43  delete _parlist;
    4644}
    4745
    4846// Process Single Epoch
    4947////////////////////////////////////////////////////////////////////////////
    50 t_irc t_pppFilter::processEpoch(int num) {
     48t_irc t_pppFilter::processEpoch(t_pppObsPool* obsPool) {
     49
    5150  _numSat = 0;
    5251  const double maxSolGap = 60.0;
    5352  bool setNeuNoiseToZero = false;
    5453
    55   if (num > 1) {
    56     setNeuNoiseToZero = true;
     54  if (!_parlist) {
     55    _parlist = new t_pppParlist();
    5756  }
    5857
    5958  // Vector of all Observations
    6059  // --------------------------
    61   t_pppObsPool::t_epoch *epoch = _obsPool->lastEpoch();
     60  t_pppObsPool::t_epoch *epoch = obsPool->lastEpoch();
    6261  if (!epoch) {
    6362    return failure;
    … …  
    7776  string epoTimeStr = string(_epoTime);
    7877
    79   const QMap<char, t_pppRefSat*> &refSatMap = epoch->refSatMap();
    80 
    81   const QList<char> &usedSystems = _parlist.usedSystems();
    82   //--
     78  const QList<char> &usedSystems = _parlist->usedSystems();
     79
    8380  // Set Parameters
    84   if (_parlist.set(_epoTime, allObs, refSatMap) != success) {
     81  // --------------
     82  if (_parlist->set(_epoTime, allObs) != success) {
    8583    return failure;
    8684  }
    87 
    88 #ifdef BNC_DEBUG_PPP
    89   if (OPT->_obsModelType == OPT->DCMcodeBias ||
    90       OPT->_obsModelType == OPT->DCMphaseBias) {
    91 //    _parlist.printParams(_epoTime);
    92   }
    93 #endif
    9485
    9586  // Status Vector, Variance-Covariance Matrix
    … …  
    9990  setStateVectorAndVarCovMatrix(xFltOld, QFltOld, setNeuNoiseToZero);
    10091
    101   // Pre-Process Satellite Systems separately
    102   // ----------------------------------------
    103   bool preProcessing = false;
    104   if (OPT->_obsModelType == OPT->DCMcodeBias ||
    105       OPT->_obsModelType == OPT->DCMphaseBias) {
    106     preProcessing = true;
    107     for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
    108       char sys = usedSystems[iSys];
    109       _refPrn.set(sys, 0);
    110       if (OPT->_refSatRequired) {
    111         _refPrn = refSatMap[sys]->prn();
    112       }
    113       vector<t_pppSatObs*> obsVector;
    114       for (unsigned jj = 0; jj < allObs.size(); jj++) {
    115         if (allObs[jj]->prn().system() == sys) {
    116           obsVector.push_back(allObs[jj]);
    117         }
    118       }
    119       if (iSys == 0) {
    120         _datumTrafo->setFirstSystem(sys);
    121       }
    122       if (processSystem(OPT->LCs(sys), obsVector, _refPrn,
    123           epoch->pseudoObsIono(), preProcessing) != success) {
    124         LOG << sys << ": processSystem !=  success (pre-processing)" << endl;
    125         _xFlt = xFltOld;
    126         _QFlt = QFltOld;
    127         restoreState(1);
    128         return failure;
    129       }
    130     }
    131     // refSat change required?
    132     // -----------------------
    133     if (_obsPool->refSatChangeRequired()) {
    134       _xFlt = xFltOld;
    135       _QFlt = QFltOld;
    136       return success;
    137     } else if (!_obsPool->refSatChangeRequired()) {
    138       initDatumTransformation(allObs, epoch->pseudoObsIono());
    139     }
    140   }
    141 
    14292  // Process Satellite Systems separately
    14393  // ------------------------------------
    144   preProcessing = false;
    14594  for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
    14695    char sys = usedSystems[iSys];
    147     _refPrn.set(sys, 0);
    148     if (OPT->_refSatRequired) {
    149       _refPrn = refSatMap[sys]->prn();
    150     }
    15196    unsigned int num = 0;
    15297    vector<t_pppSatObs*> obsVector;
    … …  
    15499      if (allObs[jj]->prn().system() == sys) {
    155100        obsVector.push_back(allObs[jj]);
    156         ++num;
    157       }
    158     }
    159     if (iSys == 0 && OPT->_obsModelType == OPT->UncombPPP) {
    160       _datumTrafo->setFirstSystem(sys);
    161     }
    162     LOG << epoTimeStr << " SATNUM " << sys << ' ' << right << setw(2) << num
    163         << endl;
    164     if (processSystem(OPT->LCs(sys), obsVector, _refPrn,
    165         epoch->pseudoObsIono(), preProcessing) != success) {
    166       LOG << "processSystem !=  success (fin-processing)" << endl;
    167       if (OPT->_obsModelType == OPT->DCMcodeBias ||
    168           OPT->_obsModelType == OPT->DCMphaseBias) {
    169         _xFlt = xFltOld;
    170         _QFlt = QFltOld;
    171         restoreState(2);
    172       }
     101        num++;
     102      }
     103    }
     104    LOG << epoTimeStr << " SATNUM " << sys << ' ' << right << setw(2) << num  << endl;
     105    if (processSystem(OPT->LCs(sys), obsVector, epoch->pseudoObsIono()) != success) {
     106      LOG << "processSystem !=  success: " << sys << endl;
    173107      return failure;
    174108    }
    … …  
    177111  // close epoch processing
    178112  // ----------------------
    179   cmpDOP(allObs, refSatMap);
    180   _parlist.printResult(_epoTime, _QFlt, _xFlt);
     113  cmpDOP(allObs);
     114  _parlist->printResult(_epoTime, _QFlt, _xFlt);
    181115  _lastEpoTimeOK = _epoTime; // remember time of last successful epoch processing
    182116  return success;
    … …  
    186120////////////////////////////////////////////////////////////////////////////
    187121t_irc t_pppFilter::processSystem(const vector<t_lc::type> &LCs,
    188     const vector<t_pppSatObs*> &obsVector, const t_prn &refPrn,
    189     bool pseudoObsIonoAvailable, bool preProcessing) {
     122                                 const vector<t_pppSatObs*> &obsVector,
     123                                 bool pseudoObsIonoAvailable) {
    190124  LOG.setf(ios::fixed);
    191   char sys = refPrn.system();
    192125
    193126  // Detect Cycle Slips
    194127  // ------------------
    195   if (detectCycleSlips(LCs, obsVector, refPrn, preProcessing) != success) {
     128  if (detectCycleSlips(LCs, obsVector) != success) {
    196129    return failure;
    197130  }
    198   if (preProcessing && _obsPool->refSatChangeRequired(sys)) {
    199     return success;
    200   }
    201 
    202   ColumnVector xSav = _xFlt;
     131
     132  ColumnVector    xSav = _xFlt;
    203133  SymmetricMatrix QSav = _QFlt;
    204   string epoTimeStr = string(_epoTime);
    205   const vector<t_pppParam*> &params = _parlist.params();
    206   unsigned nPar = _parlist.nPar();
     134  string          epoTimeStr = string(_epoTime);
     135  const vector<t_pppParam*> &params = _parlist->params();
     136  unsigned        nPar = _parlist->nPar();
    207137
    208138  unsigned usedLCs = LCs.size();
    … …  
    213143  unsigned maxObs = obsVector.size() * usedLCs;
    214144
    215   if (OPT->_pseudoObsIono && pseudoObsIonoAvailable) {
    216     maxObs -= 1; // pseudo obs iono with respect to refSat
    217   }
    218 
    219145  // Outlier Detection Loop
    220146  // ----------------------
    … …  
    229155    // -----------------------------------------------------------
    230156    Matrix AA(maxObs, nPar);
    231     ColumnVector ll(maxObs);
    232     DiagonalMatrix PP(maxObs);
    233     PP = 0.0;
     157    ColumnVector ll(maxObs);   ll = 0.0;
     158    DiagonalMatrix PP(maxObs); PP = 0.0;
    234159
    235160    int iObs = -1;
    … …  
    242167    for (unsigned ii = 0; ii < obsVector.size(); ii++) {
    243168      t_pppSatObs *obs = obsVector[ii];
    244       if (iOutlier == 0 && !preProcessing) {
     169      if (iOutlier == 0) {
    245170        obs->resetOutlier();
    246171      }
    … …  
    257182          for (unsigned iPar = 0; iPar < nPar; iPar++) {
    258183            const t_pppParam *par = params[iPar];
    259             AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
     184            AA[iObs][iPar] = par->partial(_epoTime, obs, tLC);
    260185          }
    261           ll[iObs] = obs->obsValue(tLC) - obs->cmpValue(tLC)
    262                    - DotProduct(_x0, AA.Row(iObs + 1));
     186          ll[iObs] = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA.Row(iObs + 1));
    263187          PP[iObs] = 1.0 / (obs->sigma(tLC) * obs->sigma(tLC));
    264188        }
    … …  
    269193    // ----------------------------
    270194    if ((iObs +1) < OPT->_minObs) {
    271       LOG << "t_pppFilter::processSystem not enough observations " << sys << ": " << iObs + 1 << "\n";
     195      LOG << "t_pppFilter::processSystem not enough observations " << iObs + 1 << "\n";
    272196      return failure;
    273     }
    274 
    275     if ((!iOutlier) &&
    276         (OPT->_obsModelType == OPT->DCMcodeBias ||
    277          OPT->_obsModelType == OPT->DCMphaseBias) && (!preProcessing)) {
    278       _datumTrafo->updateIndices(sys, iObs + 1);
    279       _datumTrafo->prepareAA(AA.SubMatrix(1, iObs + 1, 1, _parlist.nPar()), 1);
    280197    }
    281198
    … …  
    288205          for (unsigned jj = 0; jj < usedLCs; jj++) {
    289206            const t_lc::type tLC = LCs[jj];
    290             if (tLC == t_lc::GIM && !obs->isReference()) {
     207            if (tLC == t_lc::GIM) {
    291208              ++iObs;
    292209            } else {
    … …  
    297214            for (unsigned iPar = 0; iPar < nPar; iPar++) {
    298215              const t_pppParam *par = params[iPar];
    299               AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
     216              AA[iObs][iPar] = par->partial(_epoTime, obs, tLC);
    300217            }
    301             ll[iObs] = obs->obsValue(tLC) - obs->cmpValue(tLC)
    302                      - DotProduct(_x0, AA.Row(iObs + 1));
     218            ll[iObs] = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA.Row(iObs + 1));
    303219            PP[iObs] = 1.0 / (obs->sigma(tLC) * obs->sigma(tLC));
    304220          }
    … …  
    340256      t_pppSatObs *obs = usedObs[maxOutlierIndex];
    341257      t_pppParam *par = 0;
     258      LOG << epoTimeStr << " Outlier " << t_lc::toString(maxOutlierLC) << ' '
     259          << obs->prn().toString() << ' ' << setw(8) << setprecision(4)
     260          << maxOutlier << endl;
    342261      for (unsigned iPar = 0; iPar < nPar; iPar++) {
    343262        t_pppParam *hlp = params[iPar];
    344263        if (hlp->type() == t_pppParam::amb &&
    345             hlp->prn() == obs->prn() &&
    346             hlp->tLC() == usedTypes[maxOutlierIndex]) {
     264            hlp->prn()  == obs->prn() &&
     265            hlp->tLC()  == usedTypes[maxOutlierIndex]) {
    347266          par = hlp;
    348267        }
    349268      }
    350       if (preProcessing) {
    351         if (obs->prn() == refPrn) {
    352             LOG << epoTimeStr << " Outlier ("
    353                 << ((preProcessing) ? "pre-processing) " : "fin-processing) ")
    354                 << t_lc::toString(maxOutlierLC) << ' ' << obs->prn().toString()
    355                 << ' ' << setw(8) << setprecision(4) << maxOutlier << endl;
    356             _obsPool->setRefSatChangeRequired(sys, true);
    357             break;
     269      if (par) {
     270        if (par->ambResetCandidate()) {
     271          resetAmb(par->prn(), obsVector, maxOutlierLC, &QSav, &xSav);
    358272        }
    359273        else {
     274          par->setAmbResetCandidate();
    360275          obs->setOutlier();
    361276        }
    362277      }
    363       else {    // fin-processing
    364         LOG << epoTimeStr << " Outlier " << t_lc::toString(maxOutlierLC) << ' '
    365             << obs->prn().toString() << ' ' << setw(8) << setprecision(4)
    366             << maxOutlier << endl;
    367         if (par) {
    368           if (par->ambResetCandidate() ||
    369               OPT->_obsModelType == OPT->DCMcodeBias ||
    370               OPT->_obsModelType == OPT->DCMphaseBias) {
    371             resetAmb(par->prn(), obsVector, maxOutlierLC, &QSav, &xSav);
    372           }
    373           else {
    374             par->setAmbResetCandidate();
    375             obs->setOutlier();
    376           }
    377         }
    378         else {
    379           obs->setOutlier();
    380         }
     278      else {
     279        obs->setOutlier();
    381280      }
    382281    }
    … …  
    384283    // ---------------
    385284    else {
    386       if (!preProcessing) {
    387285        for (unsigned jj = 0; jj < LCs.size(); jj++) {
    388286          for (unsigned ii = 0; ii < usedObs.size(); ii++) {
    … …  
    393291              LOG << epoTimeStr << " RES " << left << setw(3)
    394292                  << t_lc::toString(tLC) << right << ' '
    395                   << obs->prn().toString();
    396               LOG << setw(9) << setprecision(4) << vv[ii] << endl;
    397             }
     293                  << obs->prn().toString() << ' '
     294                  << setw(8) << setprecision(4) << vv[ii] << endl;
    398295          }
    399296        }
    … …  
    408305////////////////////////////////////////////////////////////////////////////
    409306t_irc t_pppFilter::detectCycleSlips(const vector<t_lc::type> &LCs,
    410     const vector<t_pppSatObs*> &obsVector, const t_prn &refPrn,
    411     bool preProcessing) {
    412   const double SLIP = 50.0;
    413   char sys = refPrn.system();
     307                                    const vector<t_pppSatObs*> &obsVector) {
     308
     309  const double SLIP = 20.0;
    414310  string epoTimeStr = string(_epoTime);
    415   const vector<t_pppParam*> &params = _parlist.params();
    416   unsigned nPar = _parlist.nPar();
     311  const vector<t_pppParam*> &params = _parlist->params();
    417312
    418313  for (unsigned ii = 0; ii < LCs.size(); ii++) {
    … …  
    426321        bool slip = false;
    427322
    428         // in pre-processing only the reference satellite has to be checked
    429         if (preProcessing && obs->prn() != refPrn) {
    430           continue;
    431         }
    432 
    433323        if (obs->slip()) {
    434           LOG << epoTimeStr << "cycle slip set (obs) " << obs->prn().toString()
    435               << endl;
     324          LOG << epoTimeStr << "cycle slip set (obs) " << obs->prn().toString() << endl;
    436325          slip = true;
    437326        }
    438327
    439         if (_slips[obs->prn()]._obsSlipCounter != -1
    440             && _slips[obs->prn()]._obsSlipCounter != obs->slipCounter()) {
    441           LOG << epoTimeStr << "cycle slip set (obsSlipCounter) "
    442               << obs->prn().toString() << endl;
     328        if (_slips[obs->prn()]._obsSlipCounter != -1 &&
     329            _slips[obs->prn()]._obsSlipCounter != obs->slipCounter()) {
     330          LOG << epoTimeStr  << "cycle slip set (obsSlipCounter) " << obs->prn().toString()  << endl;
    443331          slip = true;
    444332        }
    445         if (!preProcessing) {
    446           _slips[obs->prn()]._obsSlipCounter = obs->slipCounter();
    447         }
    448         if (_slips[obs->prn()]._biasJumpCounter != -1
    449             && _slips[obs->prn()]._biasJumpCounter != obs->biasJumpCounter()) {
    450           LOG << epoTimeStr << "cycle slip set (biasJumpCounter) "
    451               << obs->prn().toString() << endl;
     333        _slips[obs->prn()]._obsSlipCounter = obs->slipCounter();
     334
     335        if (_slips[obs->prn()]._biasJumpCounter != -1 &&
     336            _slips[obs->prn()]._biasJumpCounter != obs->biasJumpCounter()) {
     337          LOG << epoTimeStr  << "cycle slip set (biasJumpCounter) " << obs->prn().toString() << endl;
    452338          slip = true;
    453339        }
    454         if (!preProcessing) {
    455           _slips[obs->prn()]._biasJumpCounter = obs->biasJumpCounter();
    456         }
     340        _slips[obs->prn()]._biasJumpCounter = obs->biasJumpCounter();
     341
    457342        // Slip Set
    458343        // --------
    459344        if (slip) {
    460           if (preProcessing) {
    461             _obsPool->setRefSatChangeRequired(sys, true);
    462           } else {
     345          resetAmb(obs->prn(), obsVector, tLC);
     346        }
     347
     348        // Check Pre-Fit Residuals
     349        // -----------------------
     350        else {
     351          ColumnVector AA(params.size());
     352          for (unsigned iPar = 0; iPar < params.size(); iPar++) {
     353            const t_pppParam* par = params[iPar];
     354            AA[iPar] = par->partial(_epoTime, obs, tLC);
     355          }
     356
     357          double ll = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA);
     358          double vv = DotProduct(AA, _xFlt) - ll;
     359
     360          if (fabs(vv) > SLIP) {
     361            LOG << epoTimeStr << " cycle slip detected " << t_lc::toString(tLC) << ' '
     362                << obs->prn().toString() << ' ' << setw(8) << setprecision(4) << vv << endl;
    463363            resetAmb(obs->prn(), obsVector, tLC);
    464364          }
    465365        }
    466         // Check Pre-Fit Residuals
    467         /* -----------------------
    468         else {
    469           if (!preProcessing) {
    470             ColumnVector AA(nPar);
    471             for (unsigned iPar = 0; iPar < nPar; iPar++) {
    472               const t_pppParam *par = params[iPar];
    473               AA[iPar] = par->partial(_epoTime, obs, tLC, refPrn);
    474             }
    475             double ll = obs->obsValue(tLC) - obs->cmpValue(tLC)
    476                       - DotProduct(_x0, AA);
    477             double vv = DotProduct(AA, _xFlt) - ll;
    478             if (fabs(vv) > SLIP) {
    479               LOG << epoTimeStr << " cycle slip detected " << t_lc::toString(tLC)
    480                   << ' ' << obs->prn().toString() << ' ' << setw(8)
    481                   << setprecision(4) << vv << endl;
    482               resetAmb(obs->prn(), obsVector, tLC);
    483             }
    484           }
    485         }*/
    486       }
    487     }
    488   }
     366      }
     367    }
     368  }
     369
    489370  return success;
    490371}
    … …  
    496377
    497378  t_irc irc = failure;
    498   vector<t_pppParam*> &params = _parlist.params();
     379  vector<t_pppParam*>& params = _parlist->params();
    499380  for (unsigned iPar = 0; iPar < params.size(); iPar++) {
    500381    t_pppParam *par = params[iPar];
    … …  
    506387      }
    507388      LOG << string(_epoTime) << " RESET " << par->toString() << endl;
    508       delete par;
    509       par = new t_pppParam(t_pppParam::amb, prn, tLC, &obsVector);
     389      delete par; par = new t_pppParam(t_pppParam::amb, prn, tLC, &obsVector);
    510390      par->setIndex(ind);
    511391      params[iPar] = par;
    … …  
    532412}
    533413
    534 // Add infinite noise to iono
    535 ////////////////////////////////////////////////////////////////////////////
    536 t_irc t_pppFilter::addNoiseToPar(t_pppParam::e_type parType, char sys) {
    537   t_irc irc = failure;
    538   vector<t_pppParam*> &params = _parlist.params();
    539   for (unsigned iPar = 0; iPar < params.size(); iPar++) {
    540     t_pppParam *par = params[iPar];
    541     if (par->type() == parType && par->prn().system() == sys) {
    542       int ind = par->indexNew();
    543       LOG << string(_epoTime) << " ADD NOISE TO " << par->toString() << endl;
    544       par->setIndex(ind);
    545       _QFlt(ind + 1, ind + 1) = par->sigma0() * par->sigma0();
    546       irc = success;
    547     }
    548   }
    549 
    550   return irc;
    551 }
    552 
    553414// Compute various DOP Values
    554415////////////////////////////////////////////////////////////////////////////
    555 void t_pppFilter::cmpDOP(const vector<t_pppSatObs*> &obsVector,
    556     const QMap<char, t_pppRefSat*> &refSatMap) {
     416void t_pppFilter::cmpDOP(const vector<t_pppSatObs*> &obsVector) {
    557417
    558418  _dop.reset();
    … …  
    564424    for (unsigned ii = 0; ii < obsVector.size(); ii++) {
    565425      t_pppSatObs *obs = obsVector[ii];
    566       char sys = obs->prn().system();
    567       t_prn refPrn = t_prn();
    568       if (OPT->_refSatRequired) {
    569         refPrn = refSatMap[sys]->prn();
    570       }
    571426      if (obs->isValid() && !obs->outlier()) {
    572427        ++_numSat;
    573428        for (unsigned iPar = 0; iPar < numPar; iPar++) {
    574           const t_pppParam *par = _parlist.params()[iPar];
    575           AA[_numSat - 1][iPar] = par->partial(_epoTime, obs, t_lc::c1, refPrn);
     429          const t_pppParam* par = _parlist->params()[iPar];
     430          AA[_numSat - 1][iPar] = par->partial(_epoTime, obs, t_lc::c1);
    576431        }
    577432      }
    … …  
    581436    }
    582437    AA = AA.Rows(1, _numSat);
    583     SymmetricMatrix NN;
    584     NN << AA.t() * AA;
     438    SymmetricMatrix NN; NN << AA.t() * AA;
    585439    SymmetricMatrix QQ = NN.i();
    586440
    … …  
    590444    _dop.T = sqrt(QQ(4, 4));
    591445    _dop.G = sqrt(QQ(1, 1) + QQ(2, 2) + QQ(3, 3) + QQ(4, 4));
    592   } catch (...) {
     446  }
     447  catch (...) {
    593448  }
    594449}
    … …  
    598453void t_pppFilter::predictCovCrdPart(const SymmetricMatrix &QFltOld, bool setNeuNoiseToZero) {
    599454
    600   const vector<t_pppParam*> &params = _parlist.params();
     455  const vector<t_pppParam*>& params = _parlist->params();
     456
    601457  if (params.size() < 3) {
    602458    return;
    … …  
    605461  bool first = (params[0]->indexOld() < 0);
    606462
    607   SymmetricMatrix Qneu(3);
    608   Qneu = 0.0;
     463  SymmetricMatrix Qneu(3); Qneu = 0.0;
    609464  for (unsigned ii = 0; ii < 3; ii++) {
    610465    const t_pppParam *par = params[ii];
    611466    if (first) {
    612467      Qneu[ii][ii] = par->sigma0() * par->sigma0();
    613     } else {
     468    }
     469    else {
    614470      Qneu[ii][ii] = par->noise() * par->noise();
    615471    }
    … …  
    622478  if (first) {
    623479    _QFlt.SymSubMatrix(1, 3) = Qxyz;
    624   } else {
     480  }
     481  else {
    625482    double dt = _epoTime - _firstEpoTime;
    626483    if (dt < OPT->_seedingTime || setNeuNoiseToZero) {
    627484      _QFlt.SymSubMatrix(1, 3) = QFltOld.SymSubMatrix(1, 3);
    628     } else {
     485    }
     486    else {
    629487      _QFlt.SymSubMatrix(1, 3) = QFltOld.SymSubMatrix(1, 3) + Qxyz;
    630488    }
    … …  
    637495    const SymmetricMatrix &QFltOld, bool setNeuNoiseToZero) {
    638496
    639   const vector<t_pppParam*> &params = _parlist.params();
     497  const vector<t_pppParam*>& params = _parlist->params();
    640498  unsigned nPar = params.size();
    641499
    642   _QFlt.ReSize(nPar);
    643   _QFlt = 0.0;
    644   _xFlt.ReSize(nPar);
    645   _xFlt = 0.0;
    646   _x0.ReSize(nPar);
    647   _x0 = 0.0;
     500  _QFlt.ReSize(nPar); _QFlt = 0.0;
     501  _xFlt.ReSize(nPar); _xFlt = 0.0;
     502  _x0.ReSize(nPar);   _x0   = 0.0;
    648503
    649504  for (unsigned ii = 0; ii < nPar; ii++) {
    … …  
    671526}
    672527
    673 // Compute datum transformation
    674 ////////////////////////////////////////////////////////////////////////////
    675 t_irc t_pppFilter::datumTransformation(
    676     const QMap<char, t_pppRefSat*> &refSatMap) {
    677 
    678   // get last epoch
    679   t_pppObsPool::t_epoch *epoch = _obsPool->lastEpoch();
    680 #ifdef BNC_DEBUG_PPP
    681     LOG << "GET LAST EPOCH" << endl;
    682 #endif
    683   if (!epoch) {
    684     LOG << "t_pppFilter::datumTransformation: !lastEpoch" << endl;
    685     return failure;
    686   }
    687   _epoTime = epoch->epoTime();
    688 
    689   LOG.setf(ios::fixed);
    690   LOG  << "DATUM TRANSFORMATION in Epoch  "<<  string(_epoTime) << endl;
    691 
    692   vector<t_pppSatObs*> &allObs = epoch->obsVector();
    693 
    694   const QMap<char, t_pppRefSat*> &refSatMapLastEpoch = epoch->refSatMap();
    695 
    696   bool pseudoObsIono = epoch->pseudoObsIono();
    697 
    698   // reset old and set new refSats in last epoch (ambiguities/GIM)
    699   // =============================================================
    700   if (resetRefSatellitesLastEpoch(allObs, refSatMap, refSatMapLastEpoch) != true) {
    701     LOG << "datumTransformation: resetRefSatellitesLastEpoch != success"  << endl;
    702     return failure;
    703   }
    704 
    705   if (OPT->_obsModelType == OPT->UncombPPP) {
    706     _obsPool->putEpoch(_epoTime, allObs, pseudoObsIono, refSatMap);
    707     return success;
    708   }
    709 
    710   // set AA2
    711   // =======
    712   if (_parlist.set(_epoTime, allObs, refSatMap) != success) {
    713     return failure;
    714   }
    715 #ifdef BNC_DEBUG_PPP
    716   //_parlist.printParams(_epoTime);
    717 #endif
    718 
    719   const QList<char> &usedSystems = _parlist.usedSystems();
    720   for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
    721     char sys = usedSystems[iSys];
    722     t_prn refPrn = refSatMap[sys]->prn();
    723     vector<t_pppSatObs*> obsVector;
    724     for (unsigned jj = 0; jj < allObs.size(); jj++) {
    725       if (allObs[jj]->prn().system() == sys) {
    726         allObs[jj]->resetOutlier();
    727         obsVector.push_back(allObs[jj]);
    728       }
    729     }
    730     if (iSys == 0) {
    731       _datumTrafo->setFirstSystem(sys);
    732     }
    733     vector<t_lc::type> LCs = OPT->LCs(sys);
    734     unsigned usedLCs = LCs.size();
    735     if (OPT->_pseudoObsIono && !pseudoObsIono) {
    736       usedLCs -= 1;  // GIM not used
    737     }
    738     // max Obs
    739     unsigned maxObs = obsVector.size() * usedLCs;
    740 
    741     if (OPT->_pseudoObsIono && pseudoObsIono) {
    742       maxObs -= 1; // pseudo obs iono with respect to refSat
    743     }
    744 
    745     const vector<t_pppParam*> &params = _parlist.params();
    746     unsigned nPar = _parlist.nPar();
    747 
    748     Matrix AA(maxObs, nPar); AA = 0.0;
    749 
    750     // Real Observations
    751     // -----------------
    752     int iObs = -1;
    753     for (unsigned ii = 0; ii < obsVector.size(); ii++) {
    754       t_pppSatObs *obs = obsVector[ii];
    755       for (unsigned jj = 0; jj < usedLCs; jj++) {
    756         const t_lc::type tLC = LCs[jj];
    757         if (tLC == t_lc::GIM) {
    758           continue;
    759         }
    760         ++iObs;
    761         for (unsigned iPar = 0; iPar < nPar; iPar++) {
    762           const t_pppParam *par = params[iPar];
    763           AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
    764         }
    765       }
    766     }
    767 
    768     if (!(iObs+1)) {
    769       continue;
    770     }
    771     _datumTrafo->updateIndices(sys, iObs + 1);
    772 
    773     if (_datumTrafo->prepareAA(AA.SubMatrix(1, iObs + 1, 1, nPar), 2)  != success) {
    774       return failure;
    775     }
    776   }
    777   _datumTrafo->updateNumObs();
    778 
    779   // Datum Transformation
    780   // ====================
    781   if (_datumTrafo->computeTrafoMatrix() != success) {
    782     return failure;
    783   }
    784 
    785   ColumnVector    xFltOld = _xFlt;
    786   SymmetricMatrix QFltOld = _QFlt;
    787 
    788   _QFlt << _datumTrafo->D21() * QFltOld * _datumTrafo->D21().t();
    789   _xFlt = _datumTrafo->D21() * xFltOld;
    790 
    791 #ifdef BNC_DEBUG_PPP
    792 //  LOG << "xFltOld:\n" << xFltOld << endl;
    793 //  LOG << "xFlt   :\n" << _xFlt   << endl;
    794 #endif
    795 
    796   // Reset Ambiguities after Datum Transformation
    797   // ============================================
    798   for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
    799     char sys = usedSystems[iSys];
    800     vector<t_lc::type> LCs = OPT->LCs(sys);
    801     unsigned usedLCs = LCs.size();
    802     if (OPT->_pseudoObsIono && !pseudoObsIono) {
    803       usedLCs -= 1;  // GIM not used
    804     }
    805     t_prn refPrnOld = refSatMapLastEpoch[sys]->prn();
    806     t_prn refPrnNew = refSatMap[sys]->prn();
    807     if (refPrnNew != refPrnOld) {
    808       for (unsigned jj = 0; jj < usedLCs; jj++) {
    809         const t_lc::type tLC = LCs[jj];
    810         if (tLC == t_lc::GIM) {
    811           continue;
    812         }
    813         resetAmb(refPrnOld, allObs, tLC);
    814       }
    815     }
    816   }
    817 
    818   // switch AA2 to AA1
    819   // =================
    820   _datumTrafo->switchAA();
    821 
    822   _obsPool->putEpoch(_epoTime, allObs, pseudoObsIono, refSatMap);
    823 #ifdef BNC_DEBUG_PPP
    824   LOG << "PUT EPOCH t_pppFilter::datumTransformation " << _epoTime.timestr().c_str() << endl;
    825 #endif
    826 
    827   return success;
    828 }
    829 
    830 // Init datum transformation
    831 ////////////////////////////////////////////////////////////////////////////
    832 void t_pppFilter::initDatumTransformation(
    833     const std::vector<t_pppSatObs*> &allObs, bool pseudoObsIono) {
    834   unsigned trafoObs = 0;
    835   const QList<char> &usedSystems = _parlist.usedSystems();
    836   for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
    837     char sys = usedSystems[iSys];
    838     int satNum = 0;
    839     for (unsigned jj = 0; jj < allObs.size(); jj++) {
    840       if (allObs[jj]->prn().system() == sys) {
    841         satNum++;
    842       }
    843     }
    844     // all LCs
    845     unsigned realUsedLCs = OPT->LCs(sys).size();
    846     // exclude pseudo obs GIM
    847     if (OPT->_pseudoObsIono && !pseudoObsIono) {
    848       realUsedLCs -= 1;
    849     }
    850 
    851     trafoObs += satNum * realUsedLCs;
    852 
    853     if (OPT->_pseudoObsIono && pseudoObsIono) {
    854       trafoObs -= 1; // pseudo obs iono with respect to refSat
    855     }
    856   }
    857   _datumTrafo->setNumObs(trafoObs);
    858   _datumTrafo->setNumPar(_parlist.nPar());
    859   _datumTrafo->initAA();
    860 }
    861 
    862 //
    863 //////////////////////////////////////////////////////////////////////////////
    864 bool t_pppFilter::resetRefSatellitesLastEpoch(
    865     std::vector<t_pppSatObs*> &obsVector,
    866     const QMap<char, t_pppRefSat*> &refSatMap,
    867     const QMap<char, t_pppRefSat*> &refSatMapLastEpoch) {
    868   bool resetRefSat;
    869   // reference satellite definition per system
    870   const QList<char> &usedSystems = _parlist.usedSystems();
    871   for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
    872     resetRefSat = false;
    873     char sys = usedSystems[iSys];
    874     t_prn newPrn = refSatMap[sys]->prn();
    875     t_prn oldPrn = refSatMapLastEpoch[sys]->prn();
    876 #ifdef BNC_DEBUG_PPP
    877     if (oldPrn != newPrn) {
    878       LOG << "oldRef: " << oldPrn.toString() << " => newRef " <<  newPrn.toString() << endl;
    879     }
    880 #endif
    881     vector<t_pppSatObs*>::iterator it = obsVector.begin();
    882     while (it != obsVector.end()) {
    883       t_pppSatObs *satObs = *it;
    884       if (satObs->prn() == newPrn) {
    885         resetRefSat = true;
    886         satObs->setAsReference();
    887       } else if (satObs->prn() == oldPrn) {
    888         satObs->resetReference();
    889       }
    890       it++;
    891     }
    892     if (!resetRefSat) {
    893       _obsPool->setRefSatChangeRequired(sys, true);
    894       return resetRefSat;
    895     }
    896   }
    897   return resetRefSat;
    898 }
Note: See TracChangeset for help on using the changeset viewer.