Changeset 10694 in ntrip for trunk/BNC/src/combination


Ignore:
Timestamp:
Jul 15, 2025, 4:38:52 PM (3 weeks ago)
Author:
stuerze
Message:

some corrections are added for consistency before clock combination

Location:
trunk/BNC/src/combination
Files:
2 edited

Legend:

Unmodified
Added
Removed

  • trunk/BNC/src/combination/bnccomb.cpp

    r10669 r10694  
    219219  connect(BNC_CORE, SIGNAL(newClkCorrections(QList<t_clkCorr>)), this, SLOT(slotNewClkCorrections(QList<t_clkCorr>)));
    220220  connect(BNC_CORE, SIGNAL(newCodeBiases(QList<t_satCodeBias>)), this, SLOT(slotNewCodeBiases(QList<t_satCodeBias>)));
     221  connect(BNC_CORE, SIGNAL(newPhaseBiases(QList<t_satPhaseBias>)), this, SLOT(slotNewPhaseBiases(QList<t_satPhaseBias>)));
    221222
    222223  // Combination Method
    … …  
    479480    storage[satCodeBias._prn] = satCodeBias;
    480481  }
    481 
    482 }
     482  return;
     483}
     484
     485// Remember Satellite Phase Biases
     486////////////////////////////////////////////////////////////////////////////
     487void bncComb::slotNewPhaseBiases(QList<t_satPhaseBias> satPhaseBiases) {
     488  QMutexLocker locker(&_mutex);
     489
     490  for (int ii = 0; ii < satPhaseBiases.size(); ii++) {
     491    t_satPhaseBias& satPhaseBias = satPhaseBiases[ii];
     492    QString    staID(satPhaseBias._staID.c_str());
     493    char       sys = satPhaseBias._prn.system();
     494
     495    if (!_cmbSysPrn.contains(sys)){
     496      continue;
     497    }
     498
     499    // Find/Check the AC Name
     500    // ----------------------
     501    QString acName;
     502    QStringList excludeSats;
     503    QListIterator<cmbAC*> icAC(_ACs);
     504    while (icAC.hasNext()) {
     505      cmbAC* AC = icAC.next();
     506      if (AC->mountPoint == staID) {
     507        acName      = AC->name;
     508        excludeSats = AC->excludeSats;
     509        break;
     510      }
     511    }
     512    if (acName.isEmpty() || excludeSat(satPhaseBias._prn, excludeSats)) {
     513      continue;
     514    }
     515
     516    // Store the correction
     517    // --------------------
     518    QMap<t_prn, t_satPhaseBias>& storage = _satPhaseBiases[acName];
     519    storage[satPhaseBias._prn] = satPhaseBias;
     520  }
     521  return;
     522}
     523
    483524
    484525// Process clock corrections
    … …  
    783824          }
    784825        }
    785         if (codeBiasesRefSig.size() == 2) {
    786           map<t_frequency::type, double> codeCoeff;
    787           double channel = double(_newCorr->_eph->slotNum());
    788           cmbRefSig::coeff(sys, cmbRefSig::cIF, channel, codeCoeff);
    789           map<t_frequency::type, double>::const_iterator it;
    790           for (it = codeCoeff.begin(); it != codeCoeff.end(); it++) {
    791             t_frequency::type frqType = it->first;
    792             _newCorr->_satCodeBiasIF += it->second * codeBiasesRefSig[frqType];
    793           }
     826        map<t_frequency::type, double> codeCoeff;
     827        double channel = double(_newCorr->_eph->slotNum());
     828        cmbRefSig::coeff(sys, cmbRefSig::cIF, channel, codeCoeff);
     829        map<t_frequency::type, double>::const_iterator it;
     830        for (it = codeCoeff.begin(); it != codeCoeff.end(); it++) {
     831          t_frequency::type frqType = it->first;
     832          double codeCoeff          = it->second;
     833          _newCorr->_satCodeBiasIF += codeCoeff * codeBiasesRefSig[frqType];
    794834        }
    795835        _newCorr->_satCodeBias._bias.clear();
     836      }
     837    }
     838
     839    // Check satellite phase biases
     840    // ----------------------------
     841    if (_satPhaseBiases.contains(acName)) {
     842      QMap<t_prn, t_satPhaseBias>& storage = _satPhaseBiases[acName];
     843      if (storage.contains(clkCorr._prn)) {
     844        // Yaw angle
     845        t_satPhaseBias& pb = storage[clkCorr._prn];
     846        double dt = _newCorr->_time - pb._time;
     847        if (pb._updateInt) {
     848          dt -= (0.5 * ssrUpdateInt[pb._updateInt]);
     849        }
     850        _newCorr->_satYawAngle = pb._yaw + pb._yawRate * dt;
     851
     852        // _lambdaIF
     853        map<t_frequency::type, double> codeCoeff;
     854        double channel = double(_newCorr->_eph->slotNum());
     855        cmbRefSig::coeff(sys, cmbRefSig::cIF, channel, codeCoeff);
     856        map<t_frequency::type, double>::const_iterator it;
     857        for (it = codeCoeff.begin(); it != codeCoeff.end(); it++) {
     858          t_frequency::type frqType = it->first;
     859          double codeCoeff = it->second;
     860          _newCorr->_lambdaIF += codeCoeff * t_CST::c / t_CST::freq(frqType, channel);
     861        }
    796862      }
    797863    }
    … …  
    901967  }
    902968
    903   QMap<QString, cmbCorr*> resCorr;
     969  QMap<QString, cmbCorr*> masterCorr;
    904970
    905971  // Perform the actual Combination using selected Method
    … …  
    908974  ColumnVector dx;
    909975  if (_method == filter) {
    910     irc = processEpoch_filter(epoTime, sys, out, resCorr, dx);
     976    irc = processEpoch_filter(epoTime, sys, out, masterCorr, dx);
    911977  }
    912978  else {
    913     irc = processEpoch_singleEpoch(epoTime, sys, out, resCorr, dx);
     979    irc = processEpoch_singleEpoch(epoTime, sys, out, masterCorr, dx);
    914980  }
    915981
    … …  
    921987      pp->xx += dx(iPar);
    922988      if (pp->type == cmbParam::clkSat) {
    923         if (resCorr.find(pp->prn) != resCorr.end()) {
     989        if (masterCorr.find(pp->prn) != masterCorr.end()) {
    924990          // set clock result
    925           resCorr[pp->prn]->_dClkResult = pp->xx / t_CST::c;
     991          masterCorr[pp->prn]->_dClkResult = pp->xx / t_CST::c;
    926992          // Add Code Biases from SINEX File
    927993          if (_bsx) {
    928994            map<t_frequency::type, double> codeCoeff;
    929             double channel = double(resCorr[pp->prn]->_eph->slotNum());
     995            double channel = double(masterCorr[pp->prn]->_eph->slotNum());
    930996            cmbRefSig::coeff(sys, cmbRefSig::cIF, channel, codeCoeff);
    931997            t_frequency::type fType1 = cmbRefSig::toFreq(sys, cmbRefSig::c1);
    932998            t_frequency::type fType2 = cmbRefSig::toFreq(sys, cmbRefSig::c2);
    933             _bsx->determineSsrSatCodeBiases(pp->prn.mid(0,3), codeCoeff[fType1], codeCoeff[fType2], resCorr[pp->prn]->_satCodeBias);
     999            _bsx->determineSsrSatCodeBiases(pp->prn.mid(0,3), codeCoeff[fType1], codeCoeff[fType2], masterCorr[pp->prn]->_satCodeBias);
    9341000          }
    9351001        }
    … …  
    9451011      out.flush();
    9461012    }
    947     printResults(epoTime, out, resCorr);
    948     dumpResults(epoTime, resCorr);
     1013    printResults(epoTime, out, masterCorr);
     1014    dumpResults(epoTime, masterCorr);
    9491015  }
    9501016}
    … …  
    9531019////////////////////////////////////////////////////////////////////////////
    9541020t_irc bncComb::processEpoch_filter(bncTime epoTime, char sys, QTextStream& out,
    955                                    QMap<QString, cmbCorr*>& resCorr,
     1021                                   QMap<QString, cmbCorr*>& masterCorr,
    9561022                                   ColumnVector& dx) {
    9571023
    … …  
    9761042  // Check Satellite Positions for Outliers
    9771043  // --------------------------------------
    978   if (checkOrbits(epoTime, sys, out, resCorr) != success) {
     1044  if (checkOrbits(epoTime, sys, out, masterCorr) != success) {
    9791045    return failure;
    9801046  }
    … …  
    9891055    DiagonalMatrix PP;
    9901056
    991     if (createAmat(sys, AA, ll, PP, x0, resCorr) != success) {
     1057    if (createAmat(sys, AA, ll, PP, x0) != success) {
    9921058      return failure;
    9931059    }
    … …  
    10451111////////////////////////////////////////////////////////////////////////////
    10461112void bncComb::printResults(bncTime epoTime, QTextStream& out,
    1047                            const QMap<QString, cmbCorr*>& resCorr) {
    1048 
    1049   QMapIterator<QString, cmbCorr*> it(resCorr);
     1113                           const QMap<QString, cmbCorr*>& masterCorr) {
     1114
     1115  QMapIterator<QString, cmbCorr*> it(masterCorr);
    10501116  while (it.hasNext()) {
    10511117    it.next();
    … …  
    10751141// Send results to RTNet Decoder and directly to PPP Client
    10761142////////////////////////////////////////////////////////////////////////////
    1077 void bncComb::dumpResults(bncTime epoTime, QMap<QString, cmbCorr*>& resCorr) {
     1143void bncComb::dumpResults(bncTime epoTime, QMap<QString, cmbCorr*>& masterCorr) {
    10781144
    10791145  QList<t_orbCorr> orbCorrections;
    10801146  QList<t_clkCorr> clkCorrections;
    10811147  QList<t_satCodeBias> satCodeBiasList;
     1148  QList<t_satPhaseBias> satPhaseBiasList;
    10821149
    10831150  unsigned year, month, day, hour, minute;
    … …  
    10891156                                        year, month, day, hour, minute, sec);
    10901157
    1091   QMutableMapIterator<QString, cmbCorr*> it(resCorr);
     1158  QMutableMapIterator<QString, cmbCorr*> it(masterCorr);
    10921159  while (it.hasNext()) {
    10931160    it.next();
    … …  
    11541221                  " Clk 1 %15.4f"
    11551222                  " Vel 3 %15.4f %15.4f %15.4f"
    1156                   " CoM 3 %15.4f %15.4f %15.4f",
     1223                  " CoM 3 %15.4f %15.4f %15.4f"
     1224                  " YawAngle  %1.4f",
    11571225                  apc(1), apc(2), apc(3),
    11581226                  xc(4) *  t_CST::c,
    11591227                  vv(1), vv(2), vv(3),
    1160                   com(1), com(2), com(3));
     1228                  com(1), com(2), com(3),
     1229                  corr->_satYawAngle);
    11611230    outLines += hlp;
    11621231    hlp.clear();
    … …  
    11791248      }
    11801249    }
     1250
    11811251    outLines += "\n";
    11821252    delete corr;
    … …  
    12031273// Create First Design Matrix and Vector of Measurements
    12041274////////////////////////////////////////////////////////////////////////////
    1205 t_irc bncComb::createAmat(char sys, Matrix& AA, ColumnVector& ll, DiagonalMatrix& PP,
    1206                           const ColumnVector& x0, QMap<QString, cmbCorr*>& resCorr) {
     1275t_irc bncComb::createAmat(char sys, Matrix& AA, ColumnVector& ll, DiagonalMatrix& PP, const ColumnVector& x0) {
    12071276
    12081277  unsigned nPar = _params[sys].size();
    … …  
    12331302      AA(iObs, iPar) = pp->partial(sys, corr->_acName, prn);
    12341303    }
    1235     // Consistency correction to keep the combined clock consistent to MeanOrb
    1236     // -----------------------------------------------------------------------
    1237     double dC_radial = (corr->_diffRao.t() * resCorr[prn]->_orbCorr._xr).AsScalar()
    1238                      / (resCorr[prn]->_orbCorr._xr).norm_Frobenius();
    1239 
    1240     ll(iObs) = (corr->_clkCorr._dClk * t_CST::c - corr->_satCodeBiasIF + dC_radial) - DotProduct(AA.Row(iObs), x0);
     1304    // Consistency corrections to keep the combined clock consistent to masterOrbit
     1305    // ----------------------------------------------------------------------------
     1306    double dC_orb = dotproduct(corr->_orbCorr._xr, corr->_diffRao) / corr->_orbCorr._xr.NormFrobenius();
     1307    double dC_att = corr->_diffYaw / (2 * M_PI);
     1308    dC_att *= corr->_lambdaIF;
     1309
     1310    ll(iObs) = (corr->_clkCorr._dClk * t_CST::c - corr->_satCodeBiasIF + dC_orb + dC_att) - DotProduct(AA.Row(iObs), x0);
    12411311
    12421312    PP(iObs, iObs) *= 1.0 / (corr->_weightFactor * corr->_weightFactor);
    … …  
    12831353////////////////////////////////////////////////////////////////////////////
    12841354t_irc bncComb::processEpoch_singleEpoch(bncTime epoTime, char sys, QTextStream& out,
    1285                                         QMap<QString, cmbCorr*>& resCorr,
     1355                                        QMap<QString, cmbCorr*>& masterCorr,
    12861356                                        ColumnVector& dx) {
    12871357
    12881358  // Check Satellite Positions for Outliers
    12891359  // --------------------------------------
    1290   if (checkOrbits(epoTime, sys, out, resCorr) != success) {
     1360  if (checkOrbits(epoTime, sys, out, masterCorr) != success) {
    12911361    return failure;
    12921362  }
    … …  
    13581428    ColumnVector   ll;
    13591429    DiagonalMatrix PP;
    1360     if (createAmat(sys, AA, ll, PP, x0, resCorr) != success) {
     1430    if (createAmat(sys, AA, ll, PP, x0) != success) {
    13611431      return failure;
    13621432    }
    … …  
    14121482// Check Satellite Positions for Outliers
    14131483////////////////////////////////////////////////////////////////////////////
    1414 t_irc bncComb::checkOrbits(bncTime epoTime, char sys, QTextStream& out, QMap<QString, cmbCorr*>& resCorr) {
     1484t_irc bncComb::checkOrbits(bncTime epoTime, char sys, QTextStream& out, QMap<QString, cmbCorr*>& masterCorr) {
    14151485
    14161486  // Switch to last ephemeris (if possible)
    … …  
    15051575        QString  prn  = corr->_prn;
    15061576        if (corr->_acName == _masterOrbitAC[sys] &&
    1507             resCorr.find(prn) == resCorr.end()) {
    1508           resCorr[prn] = new cmbCorr(*corr);
    1509         }
    1510       }
    1511       // Remove satellites that are not in masterOrbit
    1512       // and compute differences wrt. masterOrbit
    1513       // ----------------------------------------------
    1514       QMutableVectorIterator<cmbCorr*> im(corrs(sys));
    1515       while (im.hasNext()) {
    1516         cmbCorr* corr = im.next();
    1517         QString  prn  = corr->_prn;
    1518         if (resCorr.find(prn) == resCorr.end()) {
    1519           delete corr;
    1520           im.remove();
    1521         }
    1522         else {
    1523           corr->_diffRao = resCorr[prn]->_orbCorr._xr - corr->_orbCorr._xr;
     1577            masterCorr.find(prn) == masterCorr.end()) {
     1578          masterCorr[prn] = new cmbCorr(*corr);
     1579          masterCorr[prn]->_diffRao = 0.0;
     1580          masterCorr[prn]->_diffYaw = 0.0;
    15241581        }
    15251582      }
    … …  
    15661623        QString  prn  = corr->_prn;
    15671624        if (corr->_acName == _masterOrbitAC[sys] &&
    1568             resCorr.find(prn) == resCorr.end()) {
    1569           resCorr[prn] = new cmbCorr(*corr);
     1625            masterCorr.find(prn) == masterCorr.end()) {
     1626          masterCorr[prn] = new cmbCorr(*corr);
    15701627        }
    15711628      }
    … …  
    15771634        cmbCorr* corr = im.next();
    15781635        QString  prn  = corr->_prn;
    1579         if (resCorr.find(prn) == resCorr.end()) {
     1636        if (masterCorr.find(prn) == masterCorr.end()) {
    15801637          delete corr;
    15811638          im.remove();
    15821639        }
    15831640        else {
    1584           corr->_diffRao = resCorr[prn]->_orbCorr._xr - corr->_orbCorr._xr;
     1641          corr->_diffRao = corr->_orbCorr._xr - masterCorr[prn]->_orbCorr._xr;
     1642          corr->_diffYaw = corr->_satYawAngle - masterCorr[prn]->_satYawAngle;
    15851643        }
    15861644      }

  • trunk/BNC/src/combination/bnccomb.h

    r10665 r10694  
    1212#include <map>
    1313#include <newmat.h>
     14#include <newmatio.h>
    1415#include <deque>
    1516#include "bncephuser.h"
    … …  
    4647  void slotNewClkCorrections(QList<t_clkCorr> clkCorrections);
    4748  void slotNewCodeBiases(QList<t_satCodeBias> satCodeBiases);
     49  void slotNewPhaseBiases(QList<t_satPhaseBias> satPhaseBiases);
    4850
    4951 private slots:
    … …  
    5557  void newClkCorrections(QList<t_clkCorr>);
    5658  void newCodeBiases(QList<t_satCodeBias>);
     59  void newPhaseBiases(QList<t_satPhaseBias>);
    5760
    5861 private:
    … …  
    111114      _dClkResult                 = 0.0;
    112115      _satCodeBiasIF              = 0.0;
     116      _lambdaIF                   = 0.0;
     117      _satYawAngle                = 0.0;
    113118      _weightFactor               = 1.0;
     119      _diffRao.ReSize(3); _diffRao = 0.0;
     120      _diffYaw                    = 0.0;
    114121    }
    115122    ~cmbCorr() {
    … …  
    122129    t_clkCorr      _clkCorr;
    123130    t_satCodeBias  _satCodeBias;
     131    //t_satPhaseBias _satPhaseBias;
    124132    QString        _acName;
    125133    double         _satCodeBiasIF;
     134    double         _lambdaIF;
     135    double         _satYawAngle;
    126136    double         _dClkResult;
    127137    ColumnVector   _diffRao;
     138    double         _diffYaw;
    128139    double         _weightFactor;
    129140    QString ID() {return _acName + "_" + _prn;}
    … …  
    237248  void  processEpoch(bncTime epoTime, const std::vector<t_clkCorr>& clkCorrVec);
    238249  void  processSystem(bncTime epoTime, char sys, QTextStream& out);
    239   t_irc processEpoch_filter(bncTime epoTime, char sys, QTextStream& out, QMap<QString, cmbCorr*>& resCorr, ColumnVector& dx);
    240   t_irc processEpoch_singleEpoch(bncTime epoTime, char sys, QTextStream& out, QMap<QString, cmbCorr*>& resCorr, ColumnVector& dx);
    241   t_irc createAmat(char sys, Matrix& AA, ColumnVector& ll, DiagonalMatrix& PP,
    242                    const ColumnVector& x0, QMap<QString, cmbCorr*>& resCorr);
    243   void  dumpResults(bncTime epoTime, QMap<QString, cmbCorr*>& resCorr);
    244   void  printResults(bncTime epoTime, QTextStream& out, const QMap<QString, cmbCorr*>& resCorr);
     250  t_irc processEpoch_filter(bncTime epoTime, char sys, QTextStream& out, QMap<QString, cmbCorr*>& masterCorr, ColumnVector& dx);
     251  t_irc processEpoch_singleEpoch(bncTime epoTime, char sys, QTextStream& out, QMap<QString, cmbCorr*>& masterCorr, ColumnVector& dx);
     252  t_irc createAmat(char sys, Matrix& AA, ColumnVector& ll, DiagonalMatrix& PP, const ColumnVector& x0);
     253  void  dumpResults(bncTime epoTime, QMap<QString, cmbCorr*>& masterCorr);
     254  void  printResults(bncTime epoTime, QTextStream& out, const QMap<QString, cmbCorr*>& masterCorr);
    245255  void  switchToLastEph(t_eph* lastEph, cmbCorr* corr);
    246   t_irc checkOrbits(bncTime epoTime, char sys, QTextStream& out, QMap<QString, cmbCorr*>& resCorr);
     256  t_irc checkOrbits(bncTime epoTime, char sys, QTextStream& out, QMap<QString, cmbCorr*>& masterCorr);
    247257  bool excludeSat(const t_prn& prn, const QStringList excludeSats) const;
    248258  QVector<cmbCorr*>& corrs(char sys) {return _buffer[sys].corrs;}
    … …  
    271281  QMap<char, QVector<cmbParam*>>              _params;
    272282  QMap<QString, QMap<t_prn, t_orbCorr> >     _orbCorrections;
    273   QMap<QString, QMap<t_prn, t_satCodeBias> > _satCodeBiases;
     283  QMap<QString, QMap<t_prn, t_satCodeBias>>  _satCodeBiases;
     284  QMap<QString, QMap<t_prn, t_satPhaseBias>> _satPhaseBiases;
    274285  QMap<char, unsigned>                       _cmbSysPrn;
    275286  bncEphUser                                 _ephUser;
Note: See TracChangeset for help on using the changeset viewer.