Changeset 7203 in ntrip for trunk/BNC/src/PPP/pppClient.cpp


Ignore:
Timestamp:
Aug 17, 2015, 12:30:54 PM (9 years ago)
Author:
stuerze
Message:

some renaming regarding PPP

File:
1 edited

Legend:

Unmodified
Added
Removed

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

    r6653 r7203  
     1// Part of BNC, a utility for retrieving decoding and
     2// converting GNSS data streams from NTRIP broadcasters.
     3//
     4// Copyright (C) 2007
     5// German Federal Agency for Cartography and Geodesy (BKG)
     6// http://www.bkg.bund.de
     7// Czech Technical University Prague, Department of Geodesy
     8// http://www.fsv.cvut.cz
     9//
     10// Email: euref-ip@bkg.bund.de
     11//
     12// This program is free software; you can redistribute it and/or
     13// modify it under the terms of the GNU General Public License
     14// as published by the Free Software Foundation, version 2.
     15//
     16// This program is distributed in the hope that it will be useful,
     17// but WITHOUT ANY WARRANTY; without even the implied warranty of
     18// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
     19// GNU General Public License for more details.
     20//
     21// You should have received a copy of the GNU General Public License
     22// along with this program; if not, write to the Free Software
     23// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
     24
    125/* -------------------------------------------------------------------------
    226 * BKG NTRIP Client
    … …  
    529 * Class:      t_pppClient
    630 *
    7  * Purpose:    PPP Client processing starts here
     31 * Purpose:    Precise Point Positioning
    832 *
    933 * Author:     L. Mervart
    1034 *
    11  * Created:    29-Jul-2014
     35 * Created:    21-Nov-2009
    1236 *
    1337 * Changes:   
    … …  
    1539 * -----------------------------------------------------------------------*/
    1640
    17 #include <QThreadStorage>
    18 
    19 #include <iostream>
     41#include <newmatio.h>
    2042#include <iomanip>
    21 #include <stdlib.h>
    22 #include <string.h>
    23 #include <stdexcept>
     43#include <sstream>
    2444
    2545#include "pppClient.h"
    26 #include "pppEphPool.h"
    27 #include "pppObsPool.h"
    28 #include "bncconst.h"
     46#include "bncephuser.h"
    2947#include "bncutils.h"
    30 #include "pppStation.h"
    31 #include "bncantex.h"
    32 #include "pppFilter.h"
    3348
    3449using namespace BNC_PPP;
    3550using namespace std;
    3651
    37 // Global variable holding thread-specific pointers
     52// Constructor
     53////////////////////////////////////////////////////////////////////////////
     54t_pppClient::t_pppClient(const t_pppOptions* opt) {
     55
     56  _opt     = new t_pppOptions(*opt);
     57  _filter  = new t_pppFilter(this);
     58  _epoData = new t_epoData();
     59  _log     = new ostringstream();
     60  _ephUser = new bncEphUser(false);
     61
     62  for (unsigned ii = 0; ii <= t_prn::MAXPRN; ii++) {
     63    _satCodeBiases[ii] = 0;
     64  }
     65
     66}
     67
     68// Destructor
     69////////////////////////////////////////////////////////////////////////////
     70t_pppClient::~t_pppClient() {
     71
     72  for (unsigned ii = 0; ii <= t_prn::MAXPRN; ii++) {
     73    delete _satCodeBiases[ii];
     74  }
     75
     76  delete _filter;
     77  delete _epoData;
     78  delete _opt;
     79  delete _ephUser;
     80  delete _log;
     81}
     82
     83//
     84////////////////////////////////////////////////////////////////////////////
     85void t_pppClient::processEpoch(const vector<t_satObs*>& satObs, t_output* output) {
     86 
     87  // Convert and store observations
     88  // ------------------------------
     89  _epoData->clear();
     90  for (unsigned ii = 0; ii < satObs.size(); ii++) {
     91    const t_satObs* obs     = satObs[ii];
     92    t_prn prn = obs->_prn;
     93    if (prn.system() == 'E') {prn.setFlags(1);} // force I/NAV usage
     94    t_satData*   satData = new t_satData();
     95
     96    if (_epoData->tt.undef()) {
     97      _epoData->tt = obs->_time;
     98    }
     99
     100    satData->tt       = obs->_time;
     101    satData->prn      = QString(prn.toInternalString().c_str());
     102    satData->slipFlag = false;
     103    satData->P1       = 0.0;
     104    satData->P2       = 0.0;
     105    satData->P5       = 0.0;
     106    satData->P7       = 0.0;
     107    satData->L1       = 0.0;
     108    satData->L2       = 0.0;
     109    satData->L5       = 0.0;
     110    satData->L7       = 0.0;
     111    for (unsigned ifrq = 0; ifrq < obs->_obs.size(); ifrq++) {
     112      t_frqObs* frqObs = obs->_obs[ifrq];
     113      double cb = 0.0;
     114      const t_satCodeBias* satCB = satCodeBias(prn);
     115      if (satCB && satCB->_bias.size()) {
     116        for (unsigned ii = 0; ii < satCB->_bias.size(); ii++) {
     117
     118          const t_frqCodeBias& bias = satCB->_bias[ii];
     119          if (frqObs && frqObs->_rnxType2ch == bias._rnxType2ch) {
     120            cb  = bias._value;
     121          }
     122        }
     123      }
     124      if      (frqObs->_rnxType2ch[0] == '1') {
     125        if (frqObs->_codeValid)  satData->P1       = frqObs->_code + cb;
     126        if (frqObs->_phaseValid) satData->L1       = frqObs->_phase;
     127        if (frqObs->_slip)       satData->slipFlag = true;
     128      }
     129      else if (frqObs->_rnxType2ch[0] == '2') {
     130        if (frqObs->_codeValid)  satData->P2       = frqObs->_code + cb;
     131        if (frqObs->_phaseValid) satData->L2       = frqObs->_phase;
     132        if (frqObs->_slip)       satData->slipFlag = true;
     133      }
     134      else if (frqObs->_rnxType2ch[0] == '5') {
     135        if (frqObs->_codeValid)  satData->P5       = frqObs->_code + cb;
     136        if (frqObs->_phaseValid) satData->L5       = frqObs->_phase;
     137        if (frqObs->_slip)       satData->slipFlag = true;
     138      }
     139      else if (frqObs->_rnxType2ch[0] == '7') {
     140        if (frqObs->_codeValid)  satData->P7       = frqObs->_code + cb;
     141        if (frqObs->_phaseValid) satData->L7       = frqObs->_phase;
     142        if (frqObs->_slip)       satData->slipFlag = true;
     143      }
     144    }
     145    putNewObs(satData);
     146  }
     147
     148  // Data Pre-Processing
     149  // -------------------
     150  QMutableMapIterator<QString, t_satData*> it(_epoData->satData);
     151  while (it.hasNext()) {
     152    it.next();
     153    QString    prn     = it.key();
     154    t_satData* satData = it.value();
     155   
     156    if (cmpToT(satData) != success) {
     157      delete satData;
     158      it.remove();
     159      continue;
     160    }
     161
     162  }
     163
     164  // Filter Solution
     165  // ---------------
     166  if (_filter->update(_epoData) == success) {
     167    output->_error = false;
     168    output->_epoTime     = _filter->time();
     169    output->_xyzRover[0] = _filter->x();
     170    output->_xyzRover[1] = _filter->y();
     171    output->_xyzRover[2] = _filter->z();
     172    copy(&_filter->Q().data()[0], &_filter->Q().data()[6], output->_covMatrix);
     173    output->_neu[0]      = _filter->neu()[0];
     174    output->_neu[1]      = _filter->neu()[1];
     175    output->_neu[2]      = _filter->neu()[2];
     176    output->_numSat      = _filter->numSat();
     177    output->_pDop        = _filter->PDOP();
     178    output->_trp0        = _filter->trp0();
     179    output->_trp         = _filter->trp();
     180  }
     181  else {
     182    output->_error = true;
     183  }
     184
     185  output->_log = _log->str();
     186  delete _log; _log = new ostringstream();
     187}
     188
     189//
     190////////////////////////////////////////////////////////////////////////////
     191void t_pppClient::putNewObs(t_satData* satData) {
     192
     193  // Set Observations GPS
     194  // --------------------
     195  if      (satData->system() == 'G') {
     196    if (satData->P1 != 0.0 && satData->P2 != 0.0 &&
     197        satData->L1 != 0.0 && satData->L2 != 0.0 ) {
     198      t_frequency::type fType1 = t_lc::toFreq(satData->system(), t_lc::l1);
     199      t_frequency::type fType2 = t_lc::toFreq(satData->system(), t_lc::l2);
     200      double f1 = t_CST::freq(fType1, 0);
     201      double f2 = t_CST::freq(fType2, 0);
     202      double a1 =   f1 * f1 / (f1 * f1 - f2 * f2);
     203      double a2 = - f2 * f2 / (f1 * f1 - f2 * f2);
     204      satData->L1      = satData->L1 * t_CST::c / f1;
     205      satData->L2      = satData->L2 * t_CST::c / f2;
     206      satData->P3      = a1 * satData->P1 + a2 * satData->P2;
     207      satData->L3      = a1 * satData->L1 + a2 * satData->L2;
     208      satData->lambda3 = a1 * t_CST::c / f1 + a2 * t_CST::c / f2;
     209      satData->lkA     = a1;
     210      satData->lkB     = a2;
     211      _epoData->satData[satData->prn] = satData;
     212    }
     213    else {
     214      delete satData;
     215    }
     216  }
     217
     218  // Set Observations Glonass
     219  // ------------------------
     220  else if (satData->system() == 'R' && _opt->useSystem('R')) {
     221    if (satData->P1 != 0.0 && satData->P2 != 0.0 &&
     222        satData->L1 != 0.0 && satData->L2 != 0.0 ) {
     223
     224      int channel = 0;
     225      if (satData->system() == 'R') {
     226        const t_eph* eph = _ephUser->ephLast(satData->prn);
     227        if (eph) {
     228          channel = eph->slotNum();
     229        }
     230        else {
     231          delete satData;
     232          return;
     233        }
     234      }
     235
     236      t_frequency::type fType1 = t_lc::toFreq(satData->system(), t_lc::l1);
     237      t_frequency::type fType2 = t_lc::toFreq(satData->system(), t_lc::l2);
     238      double f1 = t_CST::freq(fType1, channel);
     239      double f2 = t_CST::freq(fType2, channel);
     240      double a1 =   f1 * f1 / (f1 * f1 - f2 * f2);
     241      double a2 = - f2 * f2 / (f1 * f1 - f2 * f2);
     242      satData->L1      = satData->L1 * t_CST::c / f1;
     243      satData->L2      = satData->L2 * t_CST::c / f2;
     244      satData->P3      = a1 * satData->P1 + a2 * satData->P2;
     245      satData->L3      = a1 * satData->L1 + a2 * satData->L2;
     246      satData->lambda3 = a1 * t_CST::c / f1 + a2 * t_CST::c / f2;
     247      satData->lkA     = a1;
     248      satData->lkB     = a2;
     249      _epoData->satData[satData->prn] = satData;
     250    }
     251    else {
     252      delete satData;
     253    }
     254  }
     255
     256  // Set Observations Galileo
     257  // ------------------------
     258  else if (satData->system() == 'E' && _opt->useSystem('E')) {
     259    if (satData->P1 != 0.0 && satData->P5 != 0.0 &&
     260        satData->L1 != 0.0 && satData->L5 != 0.0 ) {
     261      double f1 = t_CST::freq(t_frequency::E1, 0);
     262      double f5 = t_CST::freq(t_frequency::E5, 0);
     263      double a1 =   f1 * f1 / (f1 * f1 - f5 * f5);
     264      double a5 = - f5 * f5 / (f1 * f1 - f5 * f5);
     265      satData->L1      = satData->L1 * t_CST::c / f1;
     266      satData->L5      = satData->L5 * t_CST::c / f5;
     267      satData->P3      = a1 * satData->P1 + a5 * satData->P5;
     268      satData->L3      = a1 * satData->L1 + a5 * satData->L5;
     269      satData->lambda3 = a1 * t_CST::c / f1 + a5 * t_CST::c / f5;
     270      satData->lkA     = a1;
     271      satData->lkB     = a5;
     272      _epoData->satData[satData->prn] = satData;
     273    }
     274    else {
     275      delete satData;
     276    }
     277  }
     278
     279  // Set Observations BDS
     280  // ---------------------
     281  else if (satData->system() == 'C' && _opt->useSystem('C')) {
     282    if (satData->P2 != 0.0 && satData->P7 != 0.0 &&
     283        satData->L2 != 0.0 && satData->L7 != 0.0 ) {
     284      double f2 = t_CST::freq(t_frequency::C2, 0);
     285      double f7 = t_CST::freq(t_frequency::C7, 0);
     286      double a2 =   f2 * f2 / (f2 * f2 - f7 * f7);
     287      double a7 = - f7 * f7 / (f2 * f2 - f7 * f7);
     288      satData->L2      = satData->L2 * t_CST::c / f2;
     289      satData->L7      = satData->L7 * t_CST::c / f7;
     290      satData->P3      = a2 * satData->P2 + a7 * satData->P7;
     291      satData->L3      = a2 * satData->L2 + a7 * satData->L7;
     292      satData->lambda3 = a2 * t_CST::c / f2 + a7 * t_CST::c / f7;
     293      satData->lkA     = a2;
     294      satData->lkB     = a7;
     295      _epoData->satData[satData->prn] = satData;
     296    }
     297    else {
     298      delete satData;
     299    }
     300  }
     301}
     302
     303//
     304////////////////////////////////////////////////////////////////////////////
     305void t_pppClient::putOrbCorrections(const std::vector<t_orbCorr*>& corr) {
     306  for (unsigned ii = 0; ii < corr.size(); ii++) {
     307    QString prn = QString(corr[ii]->_prn.toInternalString().c_str());
     308    t_eph* eLast = _ephUser->ephLast(prn);
     309    t_eph* ePrev = _ephUser->ephPrev(prn);
     310    if      (eLast && eLast->IOD() == corr[ii]->_iod) {
     311      eLast->setOrbCorr(corr[ii]);
     312    }
     313    else if (ePrev && ePrev->IOD() == corr[ii]->_iod) {
     314      ePrev->setOrbCorr(corr[ii]);
     315    }
     316  }
     317}
     318
     319//
     320////////////////////////////////////////////////////////////////////////////
     321void t_pppClient::putClkCorrections(const std::vector<t_clkCorr*>& corr) {
     322  for (unsigned ii = 0; ii < corr.size(); ii++) {
     323    QString prn = QString(corr[ii]->_prn.toInternalString().c_str());
     324    t_eph* eLast = _ephUser->ephLast(prn);
     325    t_eph* ePrev = _ephUser->ephPrev(prn);
     326    if      (eLast && eLast->IOD() == corr[ii]->_iod) {
     327      eLast->setClkCorr(corr[ii]);
     328    }
     329    else if (ePrev && ePrev->IOD() == corr[ii]->_iod) {
     330      ePrev->setClkCorr(corr[ii]);
     331    }
     332  }
     333}
     334
     335//
    38336//////////////////////////////////////////////////////////////////////////////
    39 QThreadStorage<t_pppClient*> CLIENTS;
    40 
    41 // Static function returning thread-specific pointer
    42 //////////////////////////////////////////////////////////////////////////////
    43 t_pppClient* t_pppClient::instance() {
    44   return CLIENTS.localData();
    45 }
    46 
    47 // Constructor
    48 //////////////////////////////////////////////////////////////////////////////
    49 t_pppClient::t_pppClient(const t_pppOptions* opt) {
    50   _output   = 0;
    51   _opt      = new t_pppOptions(*opt);
    52   _log      = new ostringstream();
    53   _ephPool  = new t_pppEphPool();
    54   _obsPool  = new t_pppObsPool();
    55   _staRover = new t_pppStation();
    56   _filter   = new t_pppFilter();
    57   _tides    = new t_tides();
    58 
    59   if (!_opt->_antexFileName.empty()) {
    60     _antex = new bncAntex(_opt->_antexFileName.c_str());
    61   }
    62   else {
    63     _antex = 0;
    64   }
    65 
    66   CLIENTS.setLocalData(this);  // CLIENTS takes ownership over "this"
    67 }
    68 
    69 // Destructor
    70 //////////////////////////////////////////////////////////////////////////////
    71 t_pppClient::~t_pppClient() {
    72   delete _log;
    73   delete _opt;
    74   delete _ephPool;
    75   delete _obsPool;
    76   delete _staRover;
    77   delete _antex;
    78   delete _filter;
    79   delete _tides;
    80   clearObs();
     337void t_pppClient::putCodeBiases(const std::vector<t_satCodeBias*>& satCodeBias) {
     338  for (unsigned ii = 0; ii < satCodeBias.size(); ii++) {
     339    putCodeBias(new t_satCodeBias(*satCodeBias[ii]));
     340  }
    81341}
    82342
    … …  
    84344//////////////////////////////////////////////////////////////////////////////
    85345void t_pppClient::putEphemeris(const t_eph* eph) {
     346  bool check = _opt->_realTime;
    86347  const t_ephGPS* ephGPS = dynamic_cast<const t_ephGPS*>(eph);
    87348  const t_ephGlo* ephGlo = dynamic_cast<const t_ephGlo*>(eph);
    88349  const t_ephGal* ephGal = dynamic_cast<const t_ephGal*>(eph);
     350  const t_ephBDS* ephBDS = dynamic_cast<const t_ephBDS*>(eph);
    89351  if      (ephGPS) {
    90     _ephPool->putEphemeris(new t_ephGPS(*ephGPS));
     352    _ephUser->putNewEph(new t_ephGPS(*ephGPS), check);
    91353  }
    92354  else if (ephGlo) {
    93     _ephPool->putEphemeris(new t_ephGlo(*ephGlo));
     355    _ephUser->putNewEph(new t_ephGlo(*ephGlo), check);
    94356  }
    95357  else if (ephGal) {
    96     _ephPool->putEphemeris(new t_ephGal(*ephGal));
    97   }
    98 }
    99 
    100 //
    101 //////////////////////////////////////////////////////////////////////////////
    102 void t_pppClient::putOrbCorrections(const vector<t_orbCorr*>& corr) {
    103   for (unsigned ii = 0; ii < corr.size(); ii++) {
    104     _ephPool->putOrbCorrection(new t_orbCorr(*corr[ii]));
    105   }
    106 }
    107 
    108 //
    109 //////////////////////////////////////////////////////////////////////////////
    110 void t_pppClient::putClkCorrections(const vector<t_clkCorr*>& corr) {
    111   for (unsigned ii = 0; ii < corr.size(); ii++) {
    112     _ephPool->putClkCorrection(new t_clkCorr(*corr[ii]));
    113   }
    114 }
    115 
    116 //
    117 //////////////////////////////////////////////////////////////////////////////
    118 void t_pppClient::putCodeBiases(const vector<t_satCodeBias*>& biases) {
    119   for (unsigned ii = 0; ii < biases.size(); ii++) {
    120     _obsPool->putCodeBias(new t_satCodeBias(*biases[ii]));
    121   }
    122 }
    123 
    124 //
    125 //////////////////////////////////////////////////////////////////////////////
    126 t_irc t_pppClient::prepareObs(const vector<t_satObs*>& satObs,
    127                               vector<t_pppSatObs*>& obsVector, bncTime& epoTime) {
    128   // Default
    129   // -------
    130   epoTime.reset();
    131 
    132   // Create vector of valid observations
    133   // -----------------------------------
    134   for (unsigned ii = 0; ii < satObs.size(); ii++) {
    135     char system = satObs[ii]->_prn.system();
    136     if (OPT->useSystem(system)) {
    137       t_pppSatObs* pppSatObs = new t_pppSatObs(*satObs[ii]);
    138       if (pppSatObs->isValid()) {
    139         obsVector.push_back(pppSatObs);
    140       }
    141       else {
    142         delete pppSatObs;
    143       }
    144     }
    145   }
    146 
    147   // Check whether data are synchronized, compute epoTime
    148   // ----------------------------------------------------
    149   const double MAXSYNC = 0.05; // synchronization limit
    150   double meanDt = 0.0;
    151   for (unsigned ii = 0; ii < obsVector.size(); ii++) {
    152     const t_pppSatObs* satObs = obsVector.at(ii);
    153     if (epoTime.undef()) {
    154       epoTime = satObs->time();
    155     }
    156     else {
    157       double dt = satObs->time() - epoTime;
    158       if (fabs(dt) > MAXSYNC) {
    159         LOG << "t_pppClient::prepareObs asynchronous observations" << endl;
    160         return failure;
    161       }
    162       meanDt += dt;
    163     }
    164   }
    165 
    166   if (obsVector.size() > 0) {
    167     epoTime += meanDt / obsVector.size();
    168   }
    169 
    170   return success;
    171 }
    172 
    173 // Compute the Bancroft position, check for blunders
    174 //////////////////////////////////////////////////////////////////////////////
    175 t_irc t_pppClient::cmpBancroft(const bncTime& epoTime,
    176                                   vector<t_pppSatObs*>& obsVector,
    177                                   ColumnVector& xyzc, bool print) {
    178 
    179   t_lc::type tLC = t_lc::dummy;
    180 
    181   while (true) {
    182     Matrix BB(obsVector.size(), 4);
    183     int iObs = -1;
    184     for (unsigned ii = 0; ii < obsVector.size(); ii++) {
    185       const t_pppSatObs* satObs = obsVector.at(ii);
    186       if (satObs->prn().system() == 'G') {
    187         if (tLC == t_lc::dummy) {
    188           tLC = satObs->isValid(t_lc::cIF) ? t_lc::cIF : t_lc::c1;
    189         }
    190         if ( satObs->isValid(tLC) && (!satObs->modelSet() || satObs->eleSat() >= OPT->_minEle) ) {
    191           ++iObs;   
    192           BB[iObs][0] = satObs->xc()[0];
    193           BB[iObs][1] = satObs->xc()[1];
    194           BB[iObs][2] = satObs->xc()[2];
    195           BB[iObs][3] = satObs->obsValue(tLC) - satObs->cmpValueForBanc(tLC);
    196         }
    197       }
    198     }
    199     if (iObs + 1 < OPT->_minObs) {
    200       LOG << "t_pppClient::cmpBancroft not enough observations" << endl;
     358    _ephUser->putNewEph(new t_ephGal(*ephGal), check);
     359  }
     360  else if (ephBDS) {
     361      _ephUser->putNewEph(new t_ephBDS(*ephBDS), check);
     362    }
     363}
     364
     365// Satellite Position
     366////////////////////////////////////////////////////////////////////////////
     367t_irc t_pppClient::getSatPos(const bncTime& tt, const QString& prn,
     368                              ColumnVector& xc, ColumnVector& vv) {
     369
     370  t_eph* eLast = _ephUser->ephLast(prn);
     371  t_eph* ePrev = _ephUser->ephPrev(prn);
     372  if      (eLast && eLast->getCrd(tt, xc, vv, _opt->useOrbClkCorr()) == success) {
     373    return success;
     374  }
     375  else if (ePrev && ePrev->getCrd(tt, xc, vv, _opt->useOrbClkCorr()) == success) {
     376    return success;
     377  }
     378  return failure;
     379}
     380
     381// Correct Time of Transmission
     382////////////////////////////////////////////////////////////////////////////
     383t_irc t_pppClient::cmpToT(t_satData* satData) {
     384
     385  double prange = satData->P3;
     386  if (prange == 0.0) {
     387    return failure;
     388  }
     389
     390  double clkSat = 0.0;
     391  for (int ii = 1; ii <= 10; ii++) {
     392
     393    bncTime ToT = satData->tt - prange / t_CST::c - clkSat;
     394
     395    ColumnVector xc(4);
     396    ColumnVector vv(3);
     397    if (getSatPos(ToT, satData->prn, xc, vv) != success) {
    201398      return failure;
    202399    }
    203     BB = BB.Rows(1,iObs+1);
    204     bancroft(BB, xyzc);
    205 
    206     xyzc[3] /= t_CST::c;
    207 
    208     // Check Blunders
    209     // --------------
    210     const double BLUNDER = 100.0;
    211     double   maxRes      = 0.0;
    212     unsigned maxResIndex = 0;
    213     for (unsigned ii = 0; ii < obsVector.size(); ii++) {
    214       const t_pppSatObs* satObs = obsVector.at(ii);
    215       if ( satObs->isValid() && satObs->prn().system() == 'G' &&
    216            (!satObs->modelSet() || satObs->eleSat() >= OPT->_minEle) ) {
    217         ColumnVector rr = satObs->xc().Rows(1,3) - xyzc.Rows(1,3);
    218         double res = rr.norm_Frobenius() - satObs->obsValue(tLC)
    219           - (satObs->xc()[3] - xyzc[3]) * t_CST::c;
    220         if (fabs(res) > maxRes) {
    221           maxRes      = fabs(res);
    222           maxResIndex = ii;
    223         }
    224       }
    225     }
    226     if (maxRes < BLUNDER) {
    227       if (print) {
    228         LOG.setf(ios::fixed);
    229         LOG << string(epoTime) << " BANCROFT:"        << ' '
    230             << setw(14) << setprecision(3) << xyzc[0] << ' '
    231             << setw(14) << setprecision(3) << xyzc[1] << ' '
    232             << setw(14) << setprecision(3) << xyzc[2] << ' '
    233             << setw(14) << setprecision(3) << xyzc[3] * t_CST::c << endl << endl;
    234       }
    235       break;
    236     }
    237     else {
    238       t_pppSatObs* satObs = obsVector.at(maxResIndex);
    239       LOG << "t_pppClient::cmpBancroft outlier " << satObs->prn().toString()
    240           << " " << maxRes << endl;
    241       delete satObs;
    242       obsVector.erase(obsVector.begin() + maxResIndex);
    243     }
    244   }
    245 
    246   return success;
    247 }
    248 
    249 // Compute A Priori GPS-Glonass Offset
    250 //////////////////////////////////////////////////////////////////////////////
    251 double t_pppClient::cmpOffGG(vector<t_pppSatObs*>& obsVector) {
    252 
    253   t_lc::type tLC   = t_lc::dummy;
    254   double     offGG = 0.0;
    255 
    256   if (OPT->useSystem('R')) {
    257 
    258     while (obsVector.size() > 0) {
    259       offGG = 0.0;
    260       double   maxRes      = 0.0;
    261       int      maxResIndex = -1;
    262       t_prn    maxResPrn;
    263       unsigned nObs        = 0;
    264       for (unsigned ii = 0; ii < obsVector.size(); ii++) {
    265         t_pppSatObs* satObs = obsVector.at(ii);
    266         if (satObs->prn().system() == 'R') {
    267           if (tLC == t_lc::dummy) {
    268             tLC = satObs->isValid(t_lc::cIF) ? t_lc::cIF : t_lc::c1;
    269           }
    270           if (satObs->isValid(tLC) && (!satObs->modelSet() || satObs->eleSat() >= OPT->_minEle)) {
    271             double ll = satObs->obsValue(tLC) - satObs->cmpValue(tLC);
    272             ++nObs;
    273             offGG += ll;
    274             if (fabs(ll) > fabs(maxRes)) {
    275               maxRes      = ll;
    276               maxResIndex = ii;
    277               maxResPrn   = satObs->prn();
    278             }
    279           }
    280         }
    281       }
    282 
    283       if (nObs > 0) {
    284         offGG = offGG / nObs;
    285       }
    286       else {
    287         offGG = 0.0;
    288       }
    289 
    290       if (fabs(maxRes) > 1000.0) {
    291         LOG << "t_pppClient::cmpOffGG outlier " << maxResPrn.toString() << " " << maxRes << endl;
    292         obsVector.erase(obsVector.begin() + maxResIndex);
    293       }
    294       else {
    295         break;
    296       }
    297     }
    298   }
    299 
    300   return offGG;
    301 }
    302 
    303 //
    304 //////////////////////////////////////////////////////////////////////////////
    305 void t_pppClient::initOutput(t_output* output) {
    306   _output = output;
    307   _output->_numSat = 0;
    308   _output->_pDop   = 0.0;
    309   _output->_error  = false;
    310 }
    311 
    312 //
    313 //////////////////////////////////////////////////////////////////////////////
    314 void t_pppClient::clearObs() {
    315   for (unsigned ii = 0; ii < _obsRover.size(); ii++) {
    316     delete _obsRover.at(ii);
    317   }
    318   _obsRover.clear();
    319 }
    320 
    321 //
    322 //////////////////////////////////////////////////////////////////////////////
    323 void t_pppClient::finish(t_irc irc) {
    324 
    325   clearObs();
    326 
    327   _output->_epoTime = _epoTimeRover;
    328 
    329   if (irc == success) {
    330     _output->_xyzRover[0] = _staRover->xyzApr()[0] + _filter->x()[0];
    331     _output->_xyzRover[1] = _staRover->xyzApr()[1] + _filter->x()[1];
    332     _output->_xyzRover[2] = _staRover->xyzApr()[2] + _filter->x()[2];
    333 
    334     xyz2neu(_staRover->ellApr().data(), _filter->x().data(), _output->_neu);
    335 
    336     copy(&_filter->Q().data()[0], &_filter->Q().data()[6], _output->_covMatrix);
    337 
    338     _output->_trp0     = t_tropo::delay_saast(_staRover->xyzApr(), M_PI/2.0);
    339     _output->_trp      = _filter->trp();
    340     _output->_trpStdev = _filter->trpStdev();
    341 
    342     _output->_numSat     = _filter->numSat();
    343     _output->_pDop       = _filter->PDOP();
    344     _output->_error = false;
    345   }
    346   else {
    347     _output->_error = true;
    348   }
    349   _output->_log = _log->str();
    350   delete _log; _log = new ostringstream();
    351 }
    352 
    353 //
    354 //////////////////////////////////////////////////////////////////////////////
    355 t_irc t_pppClient::cmpModel(t_pppStation* station, const ColumnVector& xyzc,
    356                                vector<t_pppSatObs*>& obsVector) {
    357 
    358   bncTime time;
    359   time = _epoTimeRover;
    360   station->setName(OPT->_roverName);
    361   station->setAntName(OPT->_antNameRover);
    362   if (OPT->xyzAprRoverSet()) {
    363     station->setXyzApr(OPT->_xyzAprRover);
    364   }
    365   else {
    366     station->setXyzApr(xyzc.Rows(1,3));
    367   }
    368   station->setNeuEcc(OPT->_neuEccRover);
    369 
    370   // Receiver Clock
    371   // --------------
    372   station->setDClk(xyzc[3]);
    373 
    374   // Tides
    375   // -----
    376   station->setTideDspl( _tides->displacement(time, station->xyzApr()) );
    377  
    378   // Observation model
    379   // -----------------
    380   vector<t_pppSatObs*>::iterator it = obsVector.begin();
    381   while (it != obsVector.end()) {
    382     t_pppSatObs* satObs = *it;
    383     if (satObs->isValid()) {
    384       satObs->cmpModel(station);
    385     }
    386     if (satObs->isValid() && satObs->eleSat() >= OPT->_minEle) {
    387       ++it;
    388     }
    389     else {
    390       delete satObs;
    391       it = obsVector.erase(it);
    392     }
    393   }
    394 
    395   return success;
    396 }
    397 
    398 //
    399 //////////////////////////////////////////////////////////////////////////////
    400 void t_pppClient::processEpoch(const vector<t_satObs*>& satObs, t_output* output) {
    401 
    402   try {
    403     initOutput(output);
    404 
    405     // Prepare Observations of the Rover
    406     // ---------------------------------   
    407     if (prepareObs(satObs, _obsRover, _epoTimeRover) != success) {
    408       return finish(failure);
    409     }
    410 
    411     LOG << "\nResults of Epoch ";
    412     if (!_epoTimeRover.undef()) LOG << string(_epoTimeRover);
    413     LOG << "\n--------------------------------------\n";
    414  
    415     for (int iter = 1; iter <= 2; iter++) {
    416       ColumnVector xyzc(4); xyzc = 0.0;
    417       bool print = (iter == 2);
    418       if (cmpBancroft(_epoTimeRover, _obsRover, xyzc, print) != success) {
    419         return finish(failure);
    420       }
    421       if (cmpModel(_staRover, xyzc, _obsRover) != success) {
    422         return finish(failure);
    423       }
    424     }
    425 
    426     _offGG = cmpOffGG(_obsRover);
    427 
    428     // Store last epoch of data
    429     // ------------------------   
    430     _obsPool->putEpoch(_epoTimeRover, _obsRover);
    431 
    432     // Process Epoch in Filter
    433     // -----------------------
    434     if (_filter->processEpoch(_obsPool) != success) {
    435       return finish(failure);
    436     }
    437   }
    438   catch (Exception& exc) {
    439     LOG << exc.what() << endl;
    440     return finish(failure);
    441   }
    442   catch (t_except msg) {
    443     LOG << msg.what() << endl;
    444     return finish(failure);
    445   }
    446   catch (const char* msg) {
    447     LOG << msg << endl;
    448     return finish(failure);
    449   }
    450   catch (...) {
    451     LOG << "unknown exception" << endl;
    452     return finish(failure);
    453   }
    454 
    455   return finish(success);
    456 }
    457 
    458 //
    459 ////////////////////////////////////////////////////////////////////////////
    460 double lorentz(const ColumnVector& aa, const ColumnVector& bb) {
    461   return aa[0]*bb[0] +  aa[1]*bb[1] +  aa[2]*bb[2] -  aa[3]*bb[3];
    462 }
    463 
    464 //
    465 ////////////////////////////////////////////////////////////////////////////
    466 void t_pppClient::bancroft(const Matrix& BBpass, ColumnVector& pos) {
    467 
    468   if (pos.Nrows() != 4) {
    469     pos.ReSize(4);
    470   }
    471   pos = 0.0;
    472 
    473   for (int iter = 1; iter <= 2; iter++) {
    474     Matrix BB = BBpass;
    475     int mm = BB.Nrows();
    476     for (int ii = 1; ii <= mm; ii++) {
    477       double xx = BB(ii,1);
    478       double yy = BB(ii,2);
    479       double traveltime = 0.072;
    480       if (iter > 1) {
    481         double zz  = BB(ii,3);
    482         double rho = sqrt( (xx-pos(1)) * (xx-pos(1)) +
    483                            (yy-pos(2)) * (yy-pos(2)) +
    484                            (zz-pos(3)) * (zz-pos(3)) );
    485         traveltime = rho / t_CST::c;
    486       }
    487       double angle = traveltime * t_CST::omega;
    488       double cosa  = cos(angle);
    489       double sina  = sin(angle);
    490       BB(ii,1) =  cosa * xx + sina * yy;
    491       BB(ii,2) = -sina * xx + cosa * yy;
    492     }
    493    
    494     Matrix BBB;
    495     if (mm > 4) {
    496       SymmetricMatrix hlp; hlp << BB.t() * BB;
    497       BBB = hlp.i() * BB.t();
    498     }
    499     else {
    500       BBB = BB.i();
    501     }
    502     ColumnVector ee(mm); ee = 1.0;
    503     ColumnVector alpha(mm); alpha = 0.0;
    504     for (int ii = 1; ii <= mm; ii++) {
    505       alpha(ii) = lorentz(BB.Row(ii).t(),BB.Row(ii).t())/2.0;
    506     }
    507     ColumnVector BBBe     = BBB * ee;
    508     ColumnVector BBBalpha = BBB * alpha;
    509     double aa = lorentz(BBBe, BBBe);
    510     double bb = lorentz(BBBe, BBBalpha)-1;
    511     double cc = lorentz(BBBalpha, BBBalpha);
    512     double root = sqrt(bb*bb-aa*cc);
    513 
    514     Matrix hlpPos(4,2);
    515     hlpPos.Column(1) = (-bb-root)/aa * BBBe + BBBalpha;
    516     hlpPos.Column(2) = (-bb+root)/aa * BBBe + BBBalpha;
    517 
    518     ColumnVector omc(2);
    519     for (int pp = 1; pp <= 2; pp++) {
    520       hlpPos(4,pp)      = -hlpPos(4,pp);
    521       omc(pp) = BB(1,4) -
    522                 sqrt( (BB(1,1)-hlpPos(1,pp)) * (BB(1,1)-hlpPos(1,pp)) +
    523                       (BB(1,2)-hlpPos(2,pp)) * (BB(1,2)-hlpPos(2,pp)) +
    524                       (BB(1,3)-hlpPos(3,pp)) * (BB(1,3)-hlpPos(3,pp)) ) -
    525                 hlpPos(4,pp);
    526     }
    527     if ( fabs(omc(1)) > fabs(omc(2)) ) {
    528       pos = hlpPos.Column(2);
    529     }
    530     else {
    531       pos = hlpPos.Column(1);
    532     }
    533   }
    534 }
    535 
     400
     401    double clkSatOld = clkSat;
     402    clkSat = xc(4);
     403
     404    if ( fabs(clkSat-clkSatOld) * t_CST::c < 1.e-4 ) {
     405      satData->xx      = xc.Rows(1,3);
     406      satData->vv      = vv;
     407      satData->clk     = clkSat * t_CST::c;
     408      return success;
     409    }
     410  }
     411
     412  return failure;
     413}
     414
     415void t_pppClient::putCodeBias(t_satCodeBias* satCodeBias) {
     416  int iPrn = satCodeBias->_prn.toInt();
     417  delete _satCodeBiases[iPrn];
     418  _satCodeBiases[iPrn] = satCodeBias;
     419}
Note: See TracChangeset for help on using the changeset viewer.