Changeset 2780 in ntrip


Ignore:
Timestamp:
Dec 12, 2010, 6:41:47 PM (13 years ago)
Author:
mervart
Message:
 
Location:
trunk/BNC
Files:
2 edited

Legend:

Unmodified
Added
Removed

  • trunk/BNC/bncmodel.cpp

    r2769 r2780  
    5757const double   MINELE_GPS       = 10.0 * M_PI / 180.0;
    5858const double   MINELE_GLO       = 10.0 * M_PI / 180.0;
     59const double   MINELE_GAL       = 10.0 * M_PI / 180.0;
    5960const double   MAXRES_CODE_GPS  = 10.0;
    6061const double   MAXRES_PHASE_GPS = 0.10;
    6162const double   MAXRES_PHASE_GLO = 0.05;
     63const double   MAXRES_CODE_GAL  = 9999.0;
     64const double   MAXRES_PHASE_GAL = 9999.10;
     65
     66const double   _sigP3_gal = 9999.0;
     67const double   _sigL3_gal = 9999.0;
    6268
    6369// Constructor
    … …  
    195201    _useGlonass = false;
    196202  }
     203
     204  _useGalileo = true; // TODO
    197205
    198206  int nextPar = 0;
    … …  
    332340      delete satData;
    333341      iGlo.remove();
     342    }
     343  }
     344
     345  QMutableMapIterator<QString, t_satData*> iGal(epoData->satDataGal);
     346  while (iGal.hasNext()) {
     347    iGal.next();
     348    QString    prn     = iGal.key();
     349    t_satData* satData = iGal.value();
     350
     351    ColumnVector rr = satData->xx - _xcBanc.Rows(1,3);
     352    double       rho = rr.norm_Frobenius();
     353
     354    double neu[3];
     355    xyz2neu(_ellBanc.data(), rr.data(), neu);
     356
     357    satData->eleSat = acos( sqrt(neu[0]*neu[0] + neu[1]*neu[1]) / rho );
     358    if (neu[2] < 0) {
     359      satData->eleSat *= -1.0;
     360    }
     361    satData->azSat  = atan2(neu[1], neu[0]);
     362
     363    if (satData->eleSat < MINELE_GAL) {
     364      delete satData;
     365      iGal.remove();
    334366    }
    335367  }
    … …  
    514546      if (par->type == bncParam::AMB_L3) {
    515547        if (epoData->satDataGPS.find(par->prn) == epoData->satDataGPS.end() &&
    516             epoData->satDataGlo.find(par->prn) == epoData->satDataGlo.end() ) {
     548            epoData->satDataGlo.find(par->prn) == epoData->satDataGlo.end() &&
     549            epoData->satDataGal.find(par->prn) == epoData->satDataGal.end() ) {
    517550          removed = true;
    518551          delete par;
    … …  
    567600      }
    568601    }
     602
     603    QMapIterator<QString, t_satData*> iGal(epoData->satDataGal);
     604    while (iGal.hasNext()) {
     605      iGal.next();
     606      QString prn        = iGal.key();
     607      t_satData* satData = iGal.value();
     608      bool    found = false;
     609      for (int iPar = 1; iPar <= _params.size(); iPar++) {
     610        if (_params[iPar-1]->type == bncParam::AMB_L3 &&
     611            _params[iPar-1]->prn == prn) {
     612          found = true;
     613          break;
     614        }
     615      }
     616      if (!found) {
     617        bncParam* par = new bncParam(bncParam::AMB_L3, _params.size()+1, prn);
     618        _params.push_back(par);
     619        par->xx = satData->L3 - cmpValue(satData, true);
     620      }
     621    }
    569622   
    570623    int nPar = _params.size();
    … …  
    631684    unsigned nObs = 0;
    632685    if (_usePhase) {
    633       nObs = 2 * epoData->sizeGPS() + epoData->sizeGlo();
     686      nObs = 2 * (epoData->sizeGPS() + epoData->sizeGal()) + epoData->sizeGlo();
    634687    }
    635688    else {
    636       nObs = epoData->sizeGPS();  // Glonass pseudoranges are not used
     689      nObs = epoData->sizeGPS() + epoData->sizeGal(); // Glonass code not used
    637690    }
    638691   
    … …  
    700753    }
    701754
     755    // Galileo code and (optionally) phase observations
     756    // ------------------------------------------------
     757    QMapIterator<QString, t_satData*> itGal(epoData->satDataGal);
     758    while (itGal.hasNext()) {
     759      ++iObs;
     760      itGal.next();
     761      QString    prn     = itGal.key();
     762      t_satData* satData = itGal.value();
     763   
     764      ll(iObs)      = satData->P3 - cmpValue(satData, false);
     765      PP(iObs,iObs) = 1.0 / (_sigP3_gal * _sigP3_gal);
     766      for (int iPar = 1; iPar <= _params.size(); iPar++) {
     767        AA(iObs, iPar) = _params[iPar-1]->partial(satData, false);
     768      }
     769   
     770      if (_usePhase) {
     771        ++iObs;
     772        ll(iObs)      = satData->L3 - cmpValue(satData, true);
     773        PP(iObs,iObs) = 1.0 / (_sigL3_gal * _sigL3_gal);
     774        for (int iPar = 1; iPar <= _params.size(); iPar++) {
     775          if (_params[iPar-1]->type == bncParam::AMB_L3 &&
     776              _params[iPar-1]->prn  == prn) {
     777            ll(iObs) -= _params[iPar-1]->xx;
     778          }
     779          AA(iObs, iPar) = _params[iPar-1]->partial(satData, true);
     780        }
     781      }
     782    }
     783
    702784    // Compute Filter Update
    703785    // ---------------------
    … …  
    713795    ColumnVector vv_phase(epoData->sizeGPS());
    714796    ColumnVector vv_glo(epoData->sizeGlo());
     797    ColumnVector vv_gal_code(epoData->sizeGal());
     798    ColumnVector vv_gal_phase(epoData->sizeGal());
    715799
    716800    for (unsigned iobs = 1; iobs <= epoData->sizeGPS(); ++iobs) {
    … …  
    728812      }
    729813    }
     814    if (_useGalileo) {
     815      for (unsigned iobs = 1; iobs <= epoData->sizeGal(); ++iobs) {
     816        if (_usePhase) {
     817          vv_gal_code(iobs)  = vv(2*iobs-1);
     818          vv_gal_phase(iobs) = vv(2*iobs);
     819        }
     820        else {
     821          vv_gal_code(iobs)  = vv(iobs);
     822        }
     823      }
     824    }
    730825
    731826    strA   << "residuals code  " << setw(8) << setprecision(3) << vv_code.t();
    … …  
    736831      strA << "residuals glo   " << setw(8) << setprecision(3) << vv_glo.t();
    737832    }
     833    if (_useGalileo) {
     834      strA   << "res Galileo P " << setw(8) << setprecision(3) << vv_gal_code.t();
     835      if (_usePhase) {
     836        strA << "res Galileo C " << setw(8) << setprecision(3) << vv_gal_phase.t();
     837      }
     838    }
    738839    _log += strA.str().c_str();
    739840
    740841  } while (outlierDetection(QQsav, vv, epoData->satDataGPS,
    741                             epoData->satDataGlo) != 0);
     842                            epoData->satDataGlo, epoData->satDataGal) != 0);
    742843
    743844  // Remember the Epoch-specific Results for the computation of means
    … …  
    9741075                               const ColumnVector& vv,
    9751076                               QMap<QString, t_satData*>& satDataGPS,
    976                                QMap<QString, t_satData*>& satDataGlo) {
     1077                               QMap<QString, t_satData*>& satDataGlo,
     1078                               QMap<QString, t_satData*>& satDataGal) {
    9771079
    9781080  double vvMaxCodeGPS  = 0.0;
    9791081  double vvMaxPhaseGPS = 0.0;
    9801082  double vvMaxPhaseGlo = 0.0;
     1083  double vvMaxCodeGal  = 0.0;
     1084  double vvMaxPhaseGal = 0.0;
    9811085  QMutableMapIterator<QString, t_satData*> itMaxCodeGPS(satDataGPS);
    9821086  QMutableMapIterator<QString, t_satData*> itMaxPhaseGPS(satDataGPS);
    9831087  QMutableMapIterator<QString, t_satData*> itMaxPhaseGlo(satDataGlo);
     1088  QMutableMapIterator<QString, t_satData*> itMaxCodeGal(satDataGPS);
     1089  QMutableMapIterator<QString, t_satData*> itMaxPhaseGal(satDataGPS);
    9841090
    9851091  int ii = 0;
    … …  
    10201126  }
    10211127
     1128  // Galileo code and (optionally) phase residuals
     1129  // ---------------------------------------------
     1130  QMutableMapIterator<QString, t_satData*> itGal(satDataGal);
     1131  while (itGal.hasNext()) {
     1132    itGal.next();
     1133    ++ii;
     1134
     1135    if (vvMaxCodeGal == 0.0 || fabs(vv(ii)) > vvMaxCodeGal) {
     1136      vvMaxCodeGal    = fabs(vv(ii));
     1137      itMaxCodeGal = itGal;
     1138    }
     1139
     1140    if (_usePhase) {
     1141      ++ii;
     1142      if (vvMaxPhaseGal == 0.0 || fabs(vv(ii)) > vvMaxPhaseGal) {
     1143        vvMaxPhaseGal    = fabs(vv(ii));
     1144        itMaxPhaseGal = itGal;
     1145      }
     1146    }
     1147  }
     1148 
    10221149  if (vvMaxPhaseGlo > MAXRES_PHASE_GLO) {
    10231150    QString    prn     = itMaxPhaseGlo.key();
    … …  
    10541181    _log += "Outlier Phase " + prn.toAscii() + " "
    10551182          + QByteArray::number(vvMaxPhaseGPS, 'f', 3)  + "\n";
     1183
     1184    return 1;
     1185  }
     1186
     1187  else if (vvMaxCodeGal > MAXRES_CODE_GAL) {
     1188    QString    prn     = itMaxCodeGal.key();
     1189    t_satData* satData = itMaxCodeGal.value();
     1190    delete satData;
     1191    itMaxCodeGal.remove();
     1192    _QQ = QQsav;
     1193
     1194    _log += "Outlier Code " + prn.toAscii() + " "
     1195            + QByteArray::number(vvMaxCodeGal, 'f', 3) + "\n";
     1196
     1197    return 1;
     1198  }
     1199  else if (vvMaxPhaseGal > MAXRES_PHASE_GAL) {
     1200    QString    prn     = itMaxPhaseGal.key();
     1201    t_satData* satData = itMaxPhaseGal.value();
     1202    delete satData;
     1203    itMaxPhaseGal.remove();
     1204    _QQ = QQsav;
     1205
     1206    _log += "Outlier Phase " + prn.toAscii() + " "
     1207          + QByteArray::number(vvMaxPhaseGal, 'f', 3)  + "\n";
    10561208
    10571209    return 1;

  • trunk/BNC/bncmodel.h

    r2720 r2780  
    8585                          const ColumnVector& vv,
    8686                          QMap<QString, t_satData*>& satDataGPS,
    87                           QMap<QString, t_satData*>& satDataGlo);
     87                          QMap<QString, t_satData*>& satDataGlo,
     88                          QMap<QString, t_satData*>& satDataGal);
    8889  void writeNMEAstr(const QString& nmStr);
    8990
    … …  
    120121  QTextStream*          _nmeaStream;
    121122  bool                  _useGlonass;
     123  bool                  _useGalileo;
    122124  QMap<QString, double> _windUpTime;
    123125  QMap<QString, double> _windUpSum;
Note: See TracChangeset for help on using the changeset viewer.