source: ntrip/trunk/BNC/src/PPP/pppFilter.cpp@ 9532

/* -------------------------------------------------------------------------
 * BKG NTRIP Client
 * -------------------------------------------------------------------------
 *
 * Class:      t_pppFilter
 *
 * Purpose:    Filter Adjustment
 *
 * Author:     L. Mervart
 *
 * Created:    29-Jul-2014
 *
 * Changes:
 *
 * -----------------------------------------------------------------------*/

#include <iostream>
#include <iomanip>
#include <cmath>
#include <newmat.h>
#include <newmatio.h>
#include <newmatap.h>

#include "pppFilter.h"
#include "bncutils.h"
#include "pppParlist.h"
#include "pppObsPool.h"
#include "pppStation.h"

using namespace BNC_PPP;
using namespace std;

// Constructor
////////////////////////////////////////////////////////////////////////////
t_pppFilter::t_pppFilter(t_pppObsPool* obsPool) {
  _numSat = 0;
  _obsPool = obsPool;
  _refPrn  = t_prn();
  _datumTrafo = new t_datumTrafo();
}

// Destructor
////////////////////////////////////////////////////////////////////////////
t_pppFilter::~t_pppFilter() {
  delete _datumTrafo;
}

// Process Single Epoch
////////////////////////////////////////////////////////////////////////////
t_irc t_pppFilter::processEpoch() {
  _numSat     = 0;
  const double maxSolGap = 60.0;

  // Vector of all Observations
  // --------------------------
  t_pppObsPool::t_epoch* epoch = _obsPool->lastEpoch();
  if (!epoch) {
    return failure;
  }
  vector<t_pppSatObs*>& allObs = epoch->obsVector();

  // Time of the Epoch
  // -----------------
  _epoTime = epoch->epoTime();

  if (!_firstEpoTime.valid() ||
      !_lastEpoTimeOK.valid() ||
      (maxSolGap > 0.0 && _epoTime - _lastEpoTimeOK > maxSolGap)) {
    _firstEpoTime = _epoTime;
  }

  string epoTimeStr = string(_epoTime);

  const QMap<char, t_pppRefSat*>& refSatMap = epoch->refSatMap();

  //LOG << "processEpoch: printParams before set" << endl;
  //_parlist.printParams(_epoTime);

  //--
  // Set Parameters
  if (_parlist.set(_epoTime, allObs, refSatMap) != success) {
    return failure;
  }

  if (OPT->_obsModelType == OPT->DCMcodeBias ||
      OPT->_obsModelType == OPT->DCMphaseBias) {
#ifdef BNC_DEBUG_PPP
  //LOG << "processEpoch: printParams after set" << endl;
  //_parlist.printParams(_epoTime);
#endif
  }
  // Status Vector, Variance-Covariance Matrix
  // -----------------------------------------
  ColumnVector    xFltOld = _xFlt;
  SymmetricMatrix QFltOld = _QFlt;
  setStateVectorAndVarCovMatrix(xFltOld, QFltOld);

  // Pre-Process Satellite Systems separately
  // ----------------------------------------
  bool preProcessing = false;
  if (OPT->_obsModelType == OPT->DCMcodeBias ||
      OPT->_obsModelType == OPT->DCMphaseBias) {
    preProcessing = true;
    const QList<char>& usedSystems = _parlist.usedSystems();
    for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
      char sys = usedSystems[iSys];
      _refPrn.set(sys, 0);
      if (OPT->_refSatRequired) {
        _refPrn = refSatMap[sys]->prn();
      }
     vector<t_pppSatObs*> obsVector;
      for (unsigned jj = 0; jj < allObs.size(); jj++) {
        if (allObs[jj]->prn().system() == sys) {
          obsVector.push_back(allObs[jj]);
        }
      }
      if (iSys == 0) {
        _datumTrafo->setFirstSystem(sys);
      }
      if (processSystem(OPT->LCs(sys), obsVector, _refPrn,
                        epoch->pseudoObsIono(), preProcessing) != success) {
        _xFlt = xFltOld;
        _QFlt = QFltOld;
        _obsPool->deleteLastEpoch();
        restoreState(2);
        return failure;
      }
    }
    // refSat change required?
    // -----------------------
    if (_obsPool->refSatChangeRequired()) {
      _xFlt = xFltOld;
      _QFlt = QFltOld;
      return success;
    }
    else if (!_obsPool->refSatChangeRequired()) {
      initDatumTransformation(allObs, epoch->pseudoObsIono());
    }
  }

  // Process Satellite Systems separately
  // ------------------------------------
  preProcessing = false;
  const QList<char>& usedSystems = _parlist. usedSystems();
  for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
    char sys = usedSystems[iSys];
    _refPrn.set(sys, 0);
    if (OPT->_refSatRequired) {
      _refPrn = refSatMap[sys]->prn();
    }
    unsigned int num = 0;
    vector<t_pppSatObs*> obsVector;
    for (unsigned jj = 0; jj < allObs.size(); jj++) {
      if (allObs[jj]->prn().system() == sys) {
        obsVector.push_back(allObs[jj]);
        ++num;
      }
    }
    if (iSys == 0 && OPT->_obsModelType == OPT->UncombPPP) {
      _datumTrafo->setFirstSystem(sys);
    }
    LOG << epoTimeStr << " SATNUM " << sys << ' ' << right << setw(2) << num << endl;
    if (processSystem(OPT->LCs(sys), obsVector, _refPrn,
                      epoch->pseudoObsIono(), preProcessing) != success) {
      return failure;
    }
  }

  // close epoch processing
  // ----------------------
  cmpDOP(allObs, refSatMap);
  _parlist.printResult(_epoTime, _QFlt, _xFlt);
  _lastEpoTimeOK = _epoTime;  // remember time of last successful epoch processing
  return success;
}

// Process Selected LCs
////////////////////////////////////////////////////////////////////////////
t_irc t_pppFilter::processSystem(const vector<t_lc::type>& LCs,
                                 const vector<t_pppSatObs*>& obsVector,
                                 const t_prn& refPrn,
                                 bool pseudoObsIonoAvailable,
                                 bool preProcessing) {
  LOG.setf(ios::fixed);
  char sys = refPrn.system();

  // Detect Cycle Slips
  // ------------------
  if (detectCycleSlips(LCs, obsVector, refPrn, preProcessing) != success) {
    return failure;
  }
  if (preProcessing && _obsPool->refSatChangeRequired(sys)) {
    return success;
  }

  ColumnVector               xSav       = _xFlt;
  SymmetricMatrix            QSav       = _QFlt;
  string                     epoTimeStr = string(_epoTime);
  const vector<t_pppParam*>& params     = _parlist.params();
  unsigned nPar = _parlist.nPar();

  unsigned usedLCs     = LCs.size();
  if (OPT->_pseudoObsIono && !pseudoObsIonoAvailable) {
      usedLCs -= 1;  // GIM not used
  }
  int hlpLCs = 0;
  if (OPT->_pseudoObsTropo) {
    hlpLCs = -1;
  }
  // max Obs
  unsigned maxObs = obsVector.size() * (usedLCs + hlpLCs);
  if (OPT->_pseudoObsTropo && _datumTrafo->firstSystem(sys)) {
    maxObs += 1;
  }
  if (OPT->_pseudoObsIono && pseudoObsIonoAvailable) {
    maxObs -= 1; // pseudo obs iono with respect to refSat
  }

  // Outlier Detection Loop
  // ----------------------
  for (unsigned iOutlier = 0; iOutlier < maxObs; iOutlier++) {

    if (iOutlier > 0) {
      _xFlt = xSav;
      _QFlt = QSav;
    }

    // First-Design Matrix, Terms Observed-Computed, Weight Matrix
    // -----------------------------------------------------------
    Matrix                AA(maxObs, nPar);
    ColumnVector          ll(maxObs);
    DiagonalMatrix        PP(maxObs); PP = 0.0;

    int iObs = -1;
    vector<t_pppSatObs*> usedObs;
    vector<t_lc::type>   usedTypes;

    // Real Observations
    // =================
    for (unsigned ii = 0; ii < obsVector.size(); ii++) {
      t_pppSatObs* obs = obsVector[ii];
      if (iOutlier == 0 && !preProcessing) {
        obs->resetOutlier();
      }
      if (!obs->outlier()) {
        for (unsigned jj = 0; jj < usedLCs; jj++) {
          const t_lc::type tLC = LCs[jj];
          if (tLC == t_lc::GIM) {continue;}
          if (tLC == t_lc::Tz0) {continue;}
          ++iObs;
          usedObs.push_back(obs);
          usedTypes.push_back(tLC);
          for (unsigned iPar = 0; iPar < nPar; iPar++) {
            const t_pppParam* par = params[iPar];
            AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
          }
          ll[iObs] = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA.Row(iObs+1));
          PP[iObs] = 1.0 / (obs->sigma(tLC) * obs->sigma(tLC));
        }
      }
    }

    // pseudo Obs Tropo
    // ================
    if (OPT->_pseudoObsTropo && _datumTrafo->firstSystem(sys)) {
      bool _pseudoObsTropoConsidered = false;
      for (unsigned ii = 0; ii < obsVector.size(); ii++) {
        if (_pseudoObsTropoConsidered) {break;}
        t_pppSatObs* obs = obsVector[ii];
        for (unsigned jj = 0; jj < usedLCs; jj++) {
          const t_lc::type tLC = LCs[jj];
          if (tLC != t_lc::Tz0) {continue;}
          ++iObs;
          _pseudoObsTropoConsidered = true;
          usedObs.push_back(obs);
          usedTypes.push_back(tLC);
          for (unsigned iPar = 0; iPar < nPar; iPar++) {
            const t_pppParam* par = params[iPar];
            AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
          }
          ll[iObs] = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA.Row(iObs+1));
          PP[iObs] = 1.0 / (obs->sigma(tLC) * obs->sigma(tLC));
        }
      }
    }

    if ((!iOutlier) &&
        (OPT->_obsModelType == OPT->DCMcodeBias ||
         OPT->_obsModelType == OPT->DCMphaseBias) &&
        (!preProcessing)) {
      _datumTrafo->updateIndices(sys, iObs+1);
      _datumTrafo->prepareAA(AA.SubMatrix(1, iObs+1 , 1, _parlist.nPar()), 1);
    }

    // Pseudo Obs Iono
    // ================
    if (OPT->_pseudoObsIono && pseudoObsIonoAvailable) {
      for (unsigned ii = 0; ii < obsVector.size(); ii++) {
        t_pppSatObs* obs = obsVector[ii];
        if (!obs->outlier()) {
          for (unsigned jj = 0; jj < usedLCs; jj++) {
            const t_lc::type tLC = LCs[jj];
            if (tLC == t_lc::GIM && !obs->isReference()) {
              ++iObs;
            } else {continue;}
            usedObs.push_back(obs);
            usedTypes.push_back(tLC);
            for (unsigned iPar = 0; iPar < nPar; iPar++) {
              const t_pppParam* par = params[iPar];
              AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
            }
            ll[iObs] = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA.Row(iObs+1));
            PP[iObs] = 1.0 / (obs->sigma(tLC) * obs->sigma(tLC));
          }
        }
      }
    }

    // Check number of observations, truncate matrices
    // -----------------------------------------------
    if (iObs == -1) {
      return failure;
    }
    AA = AA.Rows(1, iObs+1);
    ll = ll.Rows(1, iObs+1);
    PP = PP.SymSubMatrix(1, iObs+1);

    // Kalman update step
    // ------------------
    kalman(AA, ll, PP, _QFlt, _xFlt);

    // Check Residuals
    // ---------------
    ColumnVector vv = AA * _xFlt - ll;
    double     maxOutlier      = 0.0;
    int        maxOutlierIndex = -1;
    t_lc::type maxOutlierLC    = t_lc::dummy;
    for (unsigned ii = 0; ii < usedObs.size(); ii++) {
      const t_lc::type tLC = usedTypes[ii];
      double res = fabs(vv[ii]);
      if (res > usedObs[ii]->maxRes(tLC)) {
        if (res > fabs(maxOutlier)) {
          maxOutlier      = vv[ii];
          maxOutlierIndex = ii;
          maxOutlierLC    = tLC;
        }
      }
    }

    // Mark outlier or break outlier detection loop
    // --------------------------------------------
    if (maxOutlierIndex > -1) {
      t_pppSatObs* obs = usedObs[maxOutlierIndex];
      t_pppParam* par = 0;
      LOG << epoTimeStr << " Outlier ("
          << ((preProcessing) ? "pre-processing) " : "fin-processing) ") << t_lc::toString(maxOutlierLC) << ' '
          << obs->prn().toString()                        << ' '
          << setw(8) << setprecision(4) << maxOutlier << endl;
      for (unsigned iPar = 0; iPar < nPar; iPar++) {
        t_pppParam* hlp = params[iPar];
        if (hlp->type() == t_pppParam::amb &&
            hlp->prn()  == obs->prn() &&
            hlp->tLC()  == usedTypes[maxOutlierIndex]) {
          par = hlp;
        }
      }
      if (preProcessing) {
        // for refSats no ambiguity parameter exists
         if ((obs->prn() == refPrn) &&
             (t_lc::toString(maxOutlierLC) == "l1" ||
              t_lc::toString(maxOutlierLC) == "l2") ) {
           _obsPool->setRefSatChangeRequired(sys, true);
           break;
         }
         else {
          if (par) {
            par->setAmbResetCandidate();
            obs->setOutlier();
          }
          else {
            obs->setOutlier();
          }
        }
      }
      else {// fin-processing
        if (par) {
          if (par->ambResetCandidate()) {
            resetAmb(par->prn(), obsVector, &QSav, &xSav);
          }
          else {
            par->setAmbResetCandidate();
            obs->setOutlier();
          }
        }
        else {
          obs->setOutlier();
        }
      }
    }
    // Print Residuals
    // ---------------
    else {
      if (!preProcessing) {
        for (unsigned jj = 0; jj < LCs.size(); jj++) {
          for (unsigned ii = 0; ii < usedObs.size(); ii++) {
            const t_lc::type tLC = usedTypes[ii];
            t_pppSatObs* obs = usedObs[ii];
            if (tLC == LCs[jj]) {
              obs->setRes(tLC, vv[ii]);
              LOG << epoTimeStr << " RES "
                  << left << setw(3) << t_lc::toString(tLC) << right << ' ';
              if (t_lc::toString(tLC) == "Tz0") {LOG << sys << "  ";}
              else {LOG << obs->prn().toString();}
              LOG << setw(9) << setprecision(4) << vv[ii] << endl;
            }
          }
        }
      }
      break;
    }
  }
  return success;
}

// Cycle-Slip Detection
////////////////////////////////////////////////////////////////////////////
t_irc t_pppFilter::detectCycleSlips(const vector<t_lc::type>& LCs,
                                    const vector<t_pppSatObs*>& obsVector,
                                    const t_prn& refPrn,
                                    bool preProcessing) {
  const double SLIP = 20.0;
  char sys = refPrn.system();
  string epoTimeStr = string(_epoTime);
  const vector<t_pppParam*>& params  = _parlist.params();
  unsigned nPar = _parlist.nPar();

  for (unsigned ii = 0; ii < LCs.size(); ii++) {
    const t_lc::type& tLC = LCs[ii];
    if (t_lc::includesPhase(tLC)) {
      for (unsigned iObs = 0; iObs < obsVector.size(); iObs++) {
        const t_pppSatObs* obs = obsVector[iObs];

        // Check set Slips and Jump Counters
        // ---------------------------------
        bool slip = false;

        // in pre-processing only the reference satellite has to be checked
        if (preProcessing && obs->prn() != refPrn) {
          continue;
        }

        if (obs->slip()) {
          LOG << epoTimeStr  << "cycle slip set (obs) " << obs->prn().toString()  << endl;
          slip = true;
        }

        if (_slips[obs->prn()]._obsSlipCounter != -1 &&
            _slips[obs->prn()]._obsSlipCounter != obs->slipCounter()) {
          LOG << epoTimeStr  << "cycle slip set (obsSlipCounter) " << obs->prn().toString()  << endl;
          slip = true;
        }
        if (!preProcessing) {
          _slips[obs->prn()]._obsSlipCounter = obs->slipCounter();
        }
        if (_slips[obs->prn()]._biasJumpCounter != -1 &&
            _slips[obs->prn()]._biasJumpCounter != obs->biasJumpCounter()) {
          LOG << epoTimeStr  << "cycle slip set (biasJumpCounter) " << obs->prn().toString() << endl;
          slip = true;
        }
        if (!preProcessing) {
          _slips[obs->prn()]._biasJumpCounter = obs->biasJumpCounter();
        }
        // Slip Set
        // --------
        if (slip) {
          if (preProcessing) {
            _obsPool->setRefSatChangeRequired(sys, true);
          }
          else {
            resetAmb(obs->prn(), obsVector);
          }
        }
        // Check Pre-Fit Residuals
        // -----------------------
        else {
          if (refPrn != t_prn()) {return success;}
          ColumnVector AA(nPar);
          for (unsigned iPar = 0; iPar < nPar; iPar++) {
            const t_pppParam* par = params[iPar];
            AA[iPar] = par->partial(_epoTime, obs, tLC, refPrn);
          }
          double ll = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA);
          double vv = DotProduct(AA, _xFlt) - ll;
          if (fabs(vv) > SLIP) {
            LOG << epoTimeStr << " cycle slip detected " << t_lc::toString(tLC) << ' '
              << obs->prn().toString() << ' ' << setw(8) << setprecision(4) << vv << endl;
            if (preProcessing) {
              _obsPool->setRefSatChangeRequired(sys, true);
            }
            else {
              resetAmb(obs->prn(), obsVector);
            }
          }
        }
      }
    }
  }
  return success;
}

// Reset Ambiguity Parameter (cycle slip)
////////////////////////////////////////////////////////////////////////////
t_irc t_pppFilter::resetAmb(t_prn prn, const vector<t_pppSatObs*>& obsVector,
                            SymmetricMatrix* QSav, ColumnVector* xSav) {

  t_irc irc = failure;
  vector<t_pppParam*>& params = _parlist.params();
  for (unsigned iPar = 0; iPar < params.size(); iPar++) {
    t_pppParam* par = params[iPar];
    if (par->type() == t_pppParam::amb && par->prn() == prn) {
      int ind = par->indexNew();
      t_lc::type tLC = par->tLC();
      LOG << string(_epoTime) << " RESET " << par->toString() << endl;
      delete par; par = new t_pppParam(t_pppParam::amb, prn, tLC, &obsVector);
      par->setIndex(ind);
      params[iPar] = par;
      for (unsigned ii = 1; ii <= params.size(); ii++) {
        _QFlt(ii, ind+1) = 0.0;
        if (QSav) {
          (*QSav)(ii, ind+1) = 0.0;
        }
      }
      _QFlt(ind+1,ind+1) = par->sigma0() * par->sigma0();
      if (QSav) {
        (*QSav)(ind+1,ind+1) = _QFlt(ind+1,ind+1);
      }
      _xFlt[ind] = 0.0;
      if (xSav) {
        (*xSav)[ind] = _xFlt[ind];
      }
      _x0[ind] = par->x0();
      irc = success;
    }
  }

  return irc;
}

// Add infinite noise to iono
////////////////////////////////////////////////////////////////////////////
t_irc t_pppFilter::addNoiseToIono(char sys) {

  t_irc irc = failure;
  vector<t_pppParam*>& params = _parlist.params();
  for (unsigned iPar = 0; iPar < params.size(); iPar++) {
    t_pppParam* par = params[iPar];
    if (par->type() == t_pppParam::ion && par->prn().system() == sys) {
      int ind = par->indexNew();
      LOG << string(_epoTime) << " ADD IONO_NOISE TO "  <<  par->prn().toString() << endl;
      par->setIndex(ind);
      _QFlt(ind+1,ind+1) += par->sigma0() * par->sigma0();
      irc = success;
    }
  }

  return irc;
}

// Compute various DOP Values
////////////////////////////////////////////////////////////////////////////
void t_pppFilter::cmpDOP(const vector<t_pppSatObs*>& obsVector,
                         const QMap<char, t_pppRefSat*>& refSatMap) {

  _dop.reset();

  try {
    const unsigned numPar = 4;
    Matrix AA(obsVector.size(), numPar);
    _numSat = 0;
    for (unsigned ii = 0; ii < obsVector.size(); ii++) {
      t_pppSatObs* obs = obsVector[ii];
      char sys = obs->prn().system();
      t_prn refPrn = t_prn();
      if (OPT->_refSatRequired) {
        refPrn = refSatMap[sys]->prn();
      }
      if (obs->isValid() && !obs->outlier()) {
        ++_numSat;
        for (unsigned iPar = 0; iPar < numPar; iPar++) {
          const t_pppParam* par = _parlist.params()[iPar];
          AA[_numSat-1][iPar] = par->partial(_epoTime, obs, t_lc::c1, refPrn);
        }
      }
    }
    if (_numSat < 4) {
      return;
    }
    AA = AA.Rows(1, _numSat);
    SymmetricMatrix NN; NN << AA.t() * AA;
    SymmetricMatrix QQ = NN.i();

    _dop.H = sqrt(QQ(1,1) + QQ(2,2));
    _dop.V = sqrt(QQ(3,3));
    _dop.P = sqrt(QQ(1,1) + QQ(2,2) + QQ(3,3));
    _dop.T = sqrt(QQ(4,4));
    _dop.G = sqrt(QQ(1,1) + QQ(2,2) + QQ(3,3) + QQ(4,4));
  }
  catch (...) {
  }
}

//
////////////////////////////////////////////////////////////////////////////
void t_pppFilter::predictCovCrdPart(const SymmetricMatrix& QFltOld) {

  const vector<t_pppParam*>& params = _parlist.params();
  if (params.size() < 3) {
    return;
  }

  bool first = (params[0]->indexOld() < 0);

  SymmetricMatrix Qneu(3); Qneu = 0.0;
  for (unsigned ii = 0; ii < 3; ii++) {
    const t_pppParam* par = params[ii];
    if (first) {
      Qneu[ii][ii] = par->sigma0() * par->sigma0();
    }
    else {
      Qneu[ii][ii] = par->noise() * par->noise();
    }
  }

  const t_pppStation* sta = PPP_CLIENT->staRover();
  SymmetricMatrix Qxyz(3);
  covariNEU_XYZ(Qneu, sta->ellApr().data(), Qxyz);

  if (first) {
    _QFlt.SymSubMatrix(1,3) = Qxyz;
  }
  else {
    double dt = _epoTime - _firstEpoTime;
    if (dt < OPT->_seedingTime) {
      _QFlt.SymSubMatrix(1,3) = QFltOld.SymSubMatrix(1,3);
    }
    else {
      _QFlt.SymSubMatrix(1,3) = QFltOld.SymSubMatrix(1,3) + Qxyz;
    }
  }
}

//
////////////////////////////////////////////////////////////////////////////
void t_pppFilter::setStateVectorAndVarCovMatrix(const ColumnVector& xFltOld,
                                                const SymmetricMatrix& QFltOld) {

  const vector<t_pppParam*>& params = _parlist.params();
  unsigned nPar = params.size();

  _QFlt.ReSize(nPar); _QFlt = 0.0;
  _xFlt.ReSize(nPar); _xFlt = 0.0;
  _x0.ReSize(nPar);   _x0   = 0.0;

  for (unsigned ii = 0; ii < nPar; ii++) {
    t_pppParam* par1 = params[ii];
    if (QFltOld.size() == 0) {
      par1->resetIndex();
    }
    _x0[ii] = par1->x0();
    int iOld = par1->indexOld();
    if (iOld < 0) {
      _QFlt[ii][ii] = par1->sigma0() * par1->sigma0(); // new parameter
    }
    else {
      _QFlt[ii][ii] = QFltOld[iOld][iOld] + par1->noise() * par1->noise();
      _xFlt[ii]     = xFltOld[iOld];
      for (unsigned jj = 0; jj < ii; jj++) {
        t_pppParam* par2 = params[jj];
        int  jOld = par2->indexOld();
        if (jOld >= 0) {
          _QFlt[ii][jj] = QFltOld(iOld+1,jOld+1);
        }
      }
    }
  }
  predictCovCrdPart(QFltOld);
}

// Compute datum transformation
////////////////////////////////////////////////////////////////////////////
t_irc t_pppFilter::datumTransformation(const QMap<char, t_pppRefSat*>& refSatMap) {

  // get last epoch
  t_pppObsPool::t_epoch* epoch = _obsPool->lastEpoch();
  if (!epoch) {
    LOG << "t_pppFilter::datumTransformation: !lastEpoch" << endl;
    return failure;
  }
  _epoTime = epoch->epoTime();
  LOG.setf(ios::fixed);
  LOG << string(_epoTime) << " DATUM TRANSFORMATION " << endl;

  vector<t_pppSatObs*>& allObs = epoch->obsVector();

  const QMap<char, t_pppRefSat*>& refSatMapLastEpoch = epoch->refSatMap();

  bool peseudoObsIono = epoch->pseudoObsIono();

  // reset old and set new refSats in last epoch (ambiguities/GIM)
  // =============================================================
  if (resetRefSatellitesLastEpoch(allObs, refSatMap, refSatMapLastEpoch) != true) {
    LOG  << "datumTransformation: refsatChange required" << endl;
    return success;
  }

  if (OPT->_obsModelType == OPT->UncombPPP) {
    return success;
  }
//  LOG << "datumTransformation: printParams before set" << endl;
//  _parlist.printParams(_epoTime);

  // set AA2
  // =======
  if (_parlist.set(_epoTime, allObs, refSatMap) != success) {
    return failure;
  }

#ifdef BNC_DEBUG_PPP
 // LOG << "datumTransformation: printParams after set" << endl;
//  _parlist.printParams(_epoTime);
#endif

  const QList<char>& usedSystems = _parlist.usedSystems();
  for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
    char sys = usedSystems[iSys];
    t_prn refPrn = refSatMap[sys]->prn();
    vector<t_pppSatObs*> obsVector;
    for (unsigned jj = 0; jj < allObs.size(); jj++) {
      if (allObs[jj]->prn().system() == sys) {
        obsVector.push_back(allObs[jj]);
      }
    }
    if (iSys == 0) {
      _datumTrafo->setFirstSystem(sys);
    }
    vector<t_lc::type> LCs = OPT->LCs(sys);
    unsigned usedLCs = LCs.size();
    if (OPT->_pseudoObsIono && !peseudoObsIono) {
        usedLCs -= 1;  // GIM not used
    }
    int hlpLCs = 0;
    if (OPT->_pseudoObsTropo) {
      hlpLCs = -1;
    }
    // max Obs
    unsigned maxObs = obsVector.size() * (usedLCs + hlpLCs);

    if (OPT->_pseudoObsTropo && _datumTrafo->firstSystem(sys)) {
      maxObs += 1;
    }
    if (OPT->_pseudoObsIono && peseudoObsIono) {
      maxObs -= 1; // pseudo obs iono with respect to refSat
    }

    const vector<t_pppParam*>& params = _parlist.params();
    unsigned nPar = _parlist.nPar();

    Matrix  AA(maxObs, nPar);

    // Real Observations
    // -----------------
    int iObs = -1;
    for (unsigned ii = 0; ii < obsVector.size(); ii++) {
      t_pppSatObs* obs = obsVector[ii];
      for (unsigned jj = 0; jj < usedLCs; jj++) {
        const t_lc::type tLC = LCs[jj];
        if (tLC == t_lc::GIM) {continue;}
        if (tLC == t_lc::Tz0) {continue;}
        ++iObs;
        for (unsigned iPar = 0; iPar < nPar; iPar++) {
          const t_pppParam* par = params[iPar];
          AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
        }
      }
    }
    // pseudo Obs Tropo
    // ================
    if (OPT->_pseudoObsTropo && _datumTrafo->firstSystem(sys)) {
      bool pseudoObsTropoConsidered = false;
      for (unsigned ii = 0; ii < obsVector.size(); ii++) {
        if (pseudoObsTropoConsidered) {break;}
        t_pppSatObs* obs = obsVector[ii];
        for (unsigned jj = 0; jj < usedLCs; jj++) {
          const t_lc::type tLC = LCs[jj];
          if (tLC != t_lc::Tz0) {continue;}
          ++iObs;
          pseudoObsTropoConsidered = true;
          for (unsigned iPar = 0; iPar < nPar; iPar++) {
            const t_pppParam* par = params[iPar];
            AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
          }
        }
      }
    }
    if (!iObs) {
      continue;
    }
    _datumTrafo->updateIndices(sys, iObs+1);    //LOG << "AA Ncols/Nrows: " << AA.Ncols() << "/" << AA.Nrows() << "  nPar: "  << nPar << endl;    //LOG << "AA.SubMatrix(1 .. " << iObs+1 << " , 1 .. " <<  nPar << ")" << endl;
    if(_datumTrafo->prepareAA(AA.SubMatrix(1, iObs+1 , 1, nPar), 2) != success) {
      return failure;
    }
  }
  _datumTrafo->updateNumObs();

  // Datum Transformation
  // ====================
#ifdef BNC_DEBUG_PPP
//      LOG << "AA1\n"; _datumTrafo->printMatrix(_datumTrafo->AA1(), _datumTrafo->numObs(), _datumTrafo->numPar());
//      LOG << "AA2\n"; _datumTrafo->printMatrix(_datumTrafo->AA2(), _datumTrafo->numObs(), _datumTrafo->numPar());
#endif
  if(_datumTrafo->computeTrafoMatrix() != success) {
    return failure;
  }
#ifdef BNC_DEBUG_PPP
      //LOG << "D21" << endl; _datumTrafo->printMatrix(_datumTrafo->D21(), _datumTrafo->numObs(), _datumTrafo->numPar());
#endif
  ColumnVector    xFltOld = _xFlt;
  SymmetricMatrix QFltOld = _QFlt;

  _QFlt << _datumTrafo->D21() * QFltOld * _datumTrafo->D21().t();
  _xFlt =  _datumTrafo->D21() * xFltOld;

#ifdef BNC_DEBUG_PPP
  //LOG << "xFltOld:\n" << xFltOld << endl;
  //LOG << "xFlt   :\n" << _xFlt   << endl;
#endif

  // Reset Ambiguities after Datum Transformation
  // ============================================
  for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
    char sys = usedSystems[iSys];
    t_prn refPrnOld = refSatMapLastEpoch[sys]->prn();
    t_prn refPrnNew = refSatMap[sys]->prn();
    if (refPrnNew != refPrnOld) {
      t_irc irc = resetAmb(refPrnOld, allObs);
      if (OPT->_obsModelType == OPT->DCMcodeBias) {
        if (irc == success) {
          addNoiseToIono(sys);
        }
      }
    }
  }

  // switch AA2 to AA1
  // =================
  _datumTrafo->switchAA();

  _obsPool->putEpoch(_epoTime, allObs, peseudoObsIono, refSatMap);

  return success;
}

// Init datum transformation
////////////////////////////////////////////////////////////////////////////
void t_pppFilter::initDatumTransformation(const std::vector<t_pppSatObs*>& allObs,
                                          bool pseudoObsIono) {
  unsigned trafoObs = 0;
  const QList<char>& usedSystems = _parlist. usedSystems();
  for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
    char sys = usedSystems[iSys];
    int satNum = 0;
    for (unsigned jj = 0; jj < allObs.size(); jj++) {
      if (allObs[jj]->prn().system() == sys) {
        satNum++;
      }
    }
    // all LCs
    unsigned realUsedLCs = OPT->LCs(sys).size();
    // exclude pseudo obs GIM
    if (OPT->_pseudoObsIono && !pseudoObsIono) {
      realUsedLCs -= 1;
    }
    if (OPT->_pseudoObsTropo) {
      realUsedLCs -= 1;
    }
    trafoObs += satNum * realUsedLCs;

    if (OPT->_pseudoObsTropo && _datumTrafo->firstSystem(sys)) {
      trafoObs += 1;
    }
    if (OPT->_pseudoObsIono && pseudoObsIono) {
      trafoObs -= 1; // pseudo obs iono with respect to refSat
    }
  }
  _datumTrafo->setNumObs(trafoObs);
  _datumTrafo->setNumPar(_parlist.nPar());
  _datumTrafo->initAA();
}

//
//////////////////////////////////////////////////////////////////////////////
bool t_pppFilter::resetRefSatellitesLastEpoch(std::vector<t_pppSatObs*>& obsVector,
    const QMap<char, t_pppRefSat*>& refSatMap,
    const QMap<char, t_pppRefSat*>& refSatMapLastEpoch) {
  bool resetRefSat;
  // reference satellite definition per system
  const QList<char>& usedSystems = _parlist.usedSystems();
  for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
    resetRefSat = false;
    char sys = usedSystems[iSys];
    t_prn newPrn = refSatMap[sys]->prn();
    t_prn oldPrn = refSatMapLastEpoch[sys]->prn();
#ifdef BNC_DEBUG_PPP
    if (oldPrn != newPrn) {
      LOG << "oldRef: " << oldPrn.toString() << " => newRef " <<  newPrn.toString() << endl;
    }
#endif
    vector<t_pppSatObs*>::iterator it = obsVector.begin();
    while (it != obsVector.end()) {
      t_pppSatObs* satObs = *it;
      if      (satObs->prn() == newPrn) {
        resetRefSat = true;
        satObs->setAsReference();
      } else if (satObs->prn() == oldPrn) {
        satObs->resetReference();
      }
     it++;
    }
    if (!resetRefSat) {
      _obsPool->setRefSatChangeRequired(sys, true);
      return resetRefSat;
    }
  }
  return resetRefSat;
}