source: ntrip/trunk/BNC/src/PPP/pppFilter.h@ 9527

#ifndef FILTER_H
#define FILTER_H

#include <vector>
#include <newmat.h>
#include "pppInclude.h"
#include "pppParlist.h"
#include "bnctime.h"
#include "t_prn.h"
#include "pppClient.h"

namespace BNC_PPP {

class t_pppParlist;
class t_pppObsPool;
class t_pppSatObs;

class t_pppFilter {
 public:
  t_pppFilter(t_pppObsPool* obsPool);
  ~t_pppFilter();

  t_irc processEpoch();

  const ColumnVector&    x() const {return _xFlt;}
  const SymmetricMatrix& Q() const {return _QFlt;}

  t_irc datumTransformation(const QMap<char, t_pppRefSat*>& refSatMap);
  void initDatumTransformation(const std::vector<t_pppSatObs*>& allObs, bool pseudoObsIono);
  unsigned setTrafoObs();
  void restoreState(int num)  {    LOG << "Restore parameter from last epoch    : _parlist = _parlist_sav ("<<  num << ")\n";
    _QFlt    = _QFlt_sav;
    _parlist = _parlist_sav;
  }
  void rememberState(int num) {    LOG << "Remember parameters from epoch before: _parlist_sav = _parlist ("<<  num << ")\n";
    _QFlt_sav    = _QFlt;
    _parlist_sav = _parlist;
  }

  int    numSat() const {return _numSat;}
  double HDOP() const {return _dop.H;}
  double HDOV() const {return _dop.V;}
  double PDOP() const {return _dop.P;}
  double GDOP() const {return _dop.G;}
  double trp() const {
    const std::vector<t_pppParam*>& par = _parlist.params();
    for (unsigned ii = 0; ii < par.size(); ++ii) {
      if (par[ii]->type() == t_pppParam::trp) {
        return x()[ii];
      }
    }
    return 0.0;
  };
  double trpStdev() const {
    const std::vector<t_pppParam*>& par = _parlist.params();
    for (unsigned ii = 0; ii < par.size(); ++ii) {
      if (par[ii]->type() == t_pppParam::trp) {
        return sqrt(Q()[ii][ii]);
      }
    }
    return 0.0;
  };

 private:
  class t_slip {
   public:
    t_slip() {
      _slip            = false;
      _obsSlipCounter  = -1;
      _biasJumpCounter = -1;
    }
    bool _slip;
    int  _obsSlipCounter;
    int  _biasJumpCounter;
  };

  class t_dop {
   public:
    t_dop() {reset();}
    void reset() {H = V = P = T = G = 0.0;}
    double H;
    double V;
    double P;
    double T;
    double G;
  };

  class t_datumTrafo {
  public:
    t_datumTrafo () {initIndices();}
    ~t_datumTrafo (){}

    void initIndices() {_firstRow = 1; _lastRow = 0;}
    void updateIndices(char sys, int maxObsSys) {
      if (firstSystem(sys)) {
        initIndices();
      }
      else {
        _firstRow = _lastRow + 1;
      }
      _lastRow += maxObsSys; //LOG << sys << " updateIndices: lastRow: " << _lastRow << "\n" ;
    };

    void setFirstSystem(char firstSys) { _firstSys = firstSys;}
    bool firstSystem(char sys) {
      if (_firstSys == sys) {
        return true;
      }
      return false;
    }
    void setNumObs(int maxObs) {_maxObs = maxObs;}
    void setNumPar(int numPar) {_numPar = numPar;}
    int numPar() {return _numPar;}
    int numObs() {return _maxObs;}
    void updateNumObs() {
      _maxObs = _lastRow;
      _AA1 = _AA1.SubMatrix(1, _lastRow, 1, _numPar);
      _AA2 = _AA2.SubMatrix(1, _lastRow, 1, _numPar);
    }

    const Matrix& AA1() {return _AA1;}
    const Matrix& AA2() {return _AA2;}
    const Matrix& D21() {return _D21;}

    void initAA() {
      _AA1.ReSize(_maxObs, _numPar); _AA1 = 0.0;
      _AA2.ReSize(_maxObs, _numPar); _AA2 = 0.0;
      _D21.ReSize(_numPar, _numPar); _D21 = 0.0;
    }
    t_irc prepareAA(const Matrix& AA, int ind) {//LOG << "prepare AA" << ind << "\n";
      Matrix* Prep = &_AA2;
      if (ind == 1) {
        Prep = &_AA1;
      }
      //LOG << "_firstRow: " << _firstRow << " _lastRow: " << _lastRow << " _numPar " << _numPar << std::endl;
      if (AA.Ncols() > _numPar) {
        LOG << "t_pppFilter::prepareAA: AA.Ncols() > _numPar: " << AA.Ncols() << " > " << _numPar << std::endl;
        return failure;
      }
      Prep->SubMatrix(_firstRow, _lastRow, 1, _numPar) << AA;
      return success;
    }
    void switchAA() {
      _AA1 = _AA2;
    }
    t_irc computeTrafoMatrix() {
      if (((_AA2.t() * _AA2)).Determinant() == 0.0) {
        LOG << "t_pppFilter::computeTrafoMatrix: (_AA2.t() * _AA2).inv() is singular" << std::endl;
        return failure;
      }
      _D21 = ((_AA2.t() * _AA2).i()) * _AA2.t() * _AA1;
      return success;
    }

    void printMatrix(const Matrix& X, int nRow, int nCol) {
      for (int rr = 0; rr < nRow; rr++) {
        for (int cc = 0; cc < nCol; cc++) {
          LOG << std::setw(6) << std::setprecision(3) << X[rr][cc] << " ;";
        }
        LOG << std::endl;
      }
      LOG << std::endl;
    }
  private:
    int               _firstRow;
    int               _lastRow;
    Matrix            _AA1;
    Matrix            _AA2;
    Matrix            _D21;
    char              _firstSys;
    int               _maxObs;
    int               _numPar;
    QMap<char, t_prn> _refSatMapPseudoObs;
  };

  t_irc processSystem(const std::vector<t_lc::type>& LCs,
                      const std::vector<t_pppSatObs*>& obsVector,
                      const t_prn& refPrn,
                      bool pseudoObsIonoAvailable,
                      bool preProcessing);

  t_irc detectCycleSlips(const std::vector<t_lc::type>& LCs,
                         const std::vector<t_pppSatObs*>& obsVector,
                         const t_prn& refPrn,
                         bool preProcessing);

  t_irc resetAmb(t_prn prn, const std::vector<t_pppSatObs*>& obsVector,
                 SymmetricMatrix* QSav = 0, ColumnVector* xSav = 0);

  void cmpDOP(const std::vector<t_pppSatObs*>& obsVector,
              const QMap<char, t_pppRefSat*>& refSatMap);

  void setStateVectorAndVarCovMatrix(const ColumnVector& xFltOld, const SymmetricMatrix& QFltOld);

  void predictCovCrdPart(const SymmetricMatrix& QFltOld);

  t_irc addNoiseToIono(char sys);

  bool resetRefSatellitesLastEpoch(std::vector<t_pppSatObs*>& obsVector,
                                   const QMap<char, t_pppRefSat*>& refSatMap,
                                   const QMap<char, t_pppRefSat*>& refSatMapLastEpoch);

  bncTime         _epoTime;
  t_pppParlist    _parlist;
  t_pppParlist    _parlist_sav;
  t_pppObsPool*   _obsPool;
  t_datumTrafo*   _datumTrafo;
  SymmetricMatrix _QFlt;
  SymmetricMatrix _QFlt_sav;
  ColumnVector    _xFlt;
  ColumnVector    _x0;
  t_slip          _slips[t_prn::MAXPRN+1];
  int             _numSat;
  t_dop           _dop;
  bncTime         _firstEpoTime;
  bncTime         _lastEpoTimeOK;
  t_prn           _refPrn;
};

}

#endif