Changeset 7287 in ntrip for trunk/BNC/src


Ignore:
Timestamp:
Sep 18, 2015, 12:38:18 AM (9 years ago)
Author:
stuerze
Message:

consideration of BDS in outlier detection method slightly changed

File:
1 edited

Legend:

Unmodified
Added
Removed

  • trunk/BNC/src/PPP_SSR_I/pppFilter.cpp

    r7235 r7287  
    3535 * Created:    01-Dec-2009
    3636 *
    37  * Changes:   
     37 * Changes:
    3838 *
    3939 * -----------------------------------------------------------------------*/
    … …  
    6666// Constructor
    6767////////////////////////////////////////////////////////////////////////////
    68 t_pppParam::t_pppParam(t_pppParam::parType typeIn, int indexIn, 
     68t_pppParam::t_pppParam(t_pppParam::parType typeIn, int indexIn,
    6969                   const QString& prnIn) {
    7070  type      = typeIn;
    … …  
    9090  // -----------
    9191  if      (type == CRD_X) {
    92     return (xx - satData->xx(1)) / satData->rho; 
     92    return (xx - satData->xx(1)) / satData->rho;
    9393  }
    9494  else if (type == CRD_Y) {
    95     return (xx - satData->xx(2)) / satData->rho; 
     95    return (xx - satData->xx(2)) / satData->rho;
    9696  }
    9797  else if (type == CRD_Z) {
    98     return (xx - satData->xx(3)) / satData->rho; 
     98    return (xx - satData->xx(3)) / satData->rho;
    9999  }
    100100
    … …  
    108108  // -----------
    109109  else if (type == TROPO) {
    110     return 1.0 / sin(satData->eleSat); 
     110    return 1.0 / sin(satData->eleSat);
    111111  }
    112112
    … …  
    238238  }
    239239
    240   _QQ.ReSize(_params.size()); 
     240  _QQ.ReSize(_params.size());
    241241  _QQ = 0.0;
    242242  for (int iPar = 1; iPar <= _params.size(); iPar++) {
    … …  
    244244    pp->xx = 0.0;
    245245    if      (pp->isCrd()) {
    246       _QQ(iPar,iPar) = OPT->_aprSigCrd(1) * OPT->_aprSigCrd(1); 
     246      _QQ(iPar,iPar) = OPT->_aprSigCrd(1) * OPT->_aprSigCrd(1);
    247247    }
    248248    else if (pp->type == t_pppParam::RECCLK) {
    249       _QQ(iPar,iPar) = OPT->_noiseClk * OPT->_noiseClk; 
     249      _QQ(iPar,iPar) = OPT->_noiseClk * OPT->_noiseClk;
    250250    }
    251251    else if (pp->type == t_pppParam::TROPO) {
    252       _QQ(iPar,iPar) = OPT->_aprSigTrp * OPT->_aprSigTrp; 
     252      _QQ(iPar,iPar) = OPT->_aprSigTrp * OPT->_aprSigTrp;
    253253      pp->xx = lastTrp;
    254254    }
    … …  
    327327
    328328  double rho0 = (satData->xx - xRec).norm_Frobenius();
    329   double dPhi = t_CST::omega * rho0 / t_CST::c; 
    330 
    331   xRec(1) = x() * cos(dPhi) - y() * sin(dPhi); 
    332   xRec(2) = y() * cos(dPhi) + x() * sin(dPhi); 
     329  double dPhi = t_CST::omega * rho0 / t_CST::c;
     330
     331  xRec(1) = x() * cos(dPhi) - y() * sin(dPhi);
     332  xRec(2) = y() * cos(dPhi) + x() * sin(dPhi);
    333333  xRec(3) = z();
    334334
    … …  
    337337  satData->rho = (satData->xx - xRec).norm_Frobenius();
    338338
    339   double tropDelay = delay_saast(satData->eleSat) + 
     339  double tropDelay = delay_saast(satData->eleSat) +
    340340                     trp() / sin(satData->eleSat);
    341341
    … …  
    364364  }
    365365  double phaseCenter = 0.0;
    366   if (_antex) { 
     366  if (_antex) {
    367367    bool found;
    368368    phaseCenter = satData->lkA * _antex->rcvCorr(OPT->_antNameRover, frqA,
    … …  
    382382  double cose = cos(satData->eleSat);
    383383  double sine = sin(satData->eleSat);
    384   antennaOffset = -OPT->_neuEccRover(1) * cosa*cose 
    385                   -OPT->_neuEccRover(2) * sina*cose 
     384  antennaOffset = -OPT->_neuEccRover(1) * cosa*cose
     385                  -OPT->_neuEccRover(2) * sina*cose
    386386                  -OPT->_neuEccRover(3) * sine;
    387387
    388   return satData->rho + phaseCenter + antennaOffset + clk() 
     388  return satData->rho + phaseCenter + antennaOffset + clk()
    389389                      + offset - satData->clk + tropDelay + wind;
    390390}
    … …  
    396396  Tracer tracer("t_pppFilter::delay_saast");
    397397
    398   double xyz[3]; 
     398  double xyz[3];
    399399  xyz[0] = x();
    400400  xyz[1] = y();
    401401  xyz[2] = z();
    402   double ell[3]; 
     402  double ell[3];
    403403  xyz2ell(xyz, ell);
    404404  double height = ell[2];
    … …  
    410410
    411411  double h_km = height / 1000.0;
    412  
     412
    413413  if (h_km < 0.0) h_km = 0.0;
    414414  if (h_km > 5.0) h_km = 5.0;
    415415  int    ii   = int(h_km + 1);
    416416  double href = ii - 1;
    417  
    418   double bCor[6]; 
     417
     418  double bCor[6];
    419419  bCor[0] = 1.156;
    420420  bCor[1] = 1.006;
    … …  
    423423  bCor[4] = 0.654;
    424424  bCor[5] = 0.563;
    425  
     425
    426426  double BB = bCor[ii-1] + (bCor[ii]-bCor[ii-1]) * (h_km - href);
    427  
     427
    428428  double zen  = M_PI/2.0 - Ele;
    429429
    … …  
    447447      reset();
    448448    }
    449    
     449
    450450    // Use different white noise for Quick-Start mode
    451451    // ----------------------------------------------
    … …  
    459459    for (int iPar = 1; iPar <= _params.size(); iPar++) {
    460460      t_pppParam* pp = _params[iPar-1];
    461    
     461
    462462      // Coordinates
    463463      // -----------
    … …  
    494494        }
    495495        _QQ(iPar,iPar) += sigCrdP_used * sigCrdP_used;
    496       }   
    497    
     496      }
     497
    498498      // Receiver Clocks
    499499      // ---------------
    … …  
    505505        _QQ(iPar,iPar) = OPT->_noiseClk * OPT->_noiseClk;
    506506      }
    507    
     507
    508508      // Tropospheric Delay
    509509      // ------------------
    … …  
    511511        _QQ(iPar,iPar) += OPT->_noiseTrp * OPT->_noiseTrp;
    512512      }
    513    
     513
    514514      // Glonass Offset
    515515      // --------------
    … …  
    531531      // ----------
    532532      else if (pp->type == t_pppParam::BDS_OFFSET) {
    533         _QQ(iPar,iPar) = 1000.0 * 1000.0;    //TODO: TEST
     533        _QQ(iPar,iPar) += 0.1 * 0.1;    //TODO: TEST
    534534      }
    535535    }
    … …  
    544544    // -----------------------------------------------
    545545    SymmetricMatrix QQ_old = _QQ;
    546    
     546
    547547    for (int iPar = 1; iPar <= _params.size(); iPar++) {
    548548      _params[iPar-1]->index_old = _params[iPar-1]->index;
    549549      _params[iPar-1]->index     = 0;
    550550    }
    551    
     551
    552552    // Remove Ambiguity Parameters without observations
    553553    // ------------------------------------------------
    … …  
    569569      }
    570570    }
    571    
     571
    572572    // Add new ambiguity parameters
    573573    // ----------------------------
    … …  
    578578      addAmb(satData);
    579579    }
    580    
     580
    581581    int nPar = _params.size();
    582582    _QQ.ReSize(nPar); _QQ = 0.0;
    … …  
    593593      }
    594594    }
    595    
     595
    596596    for (int ii = 1; ii <= nPar; ii++) {
    597597      t_pppParam* par = _params[ii-1];
    … …  
    634634    t_pppParam* par = itPar.next();
    635635    if      (par->type == t_pppParam::RECCLK) {
    636       LOG << "\n    clk     = " << setw(10) << setprecision(3) << par->xx 
    637           << " +- " << setw(6) << setprecision(3) 
     636      LOG << "\n    clk     = " << setw(10) << setprecision(3) << par->xx
     637          << " +- " << setw(6) << setprecision(3)
    638638          << sqrt(_QQ(par->index,par->index));
    639639    }
    … …  
    641641      ++par->numEpo;
    642642      LOG << "\n    amb " << par->prn.mid(0,3).toAscii().data() << " = "
    643           << setw(10) << setprecision(3) << par->xx 
    644           << " +- " << setw(6) << setprecision(3) 
     643          << setw(10) << setprecision(3) << par->xx
     644          << " +- " << setw(6) << setprecision(3)
    645645          << sqrt(_QQ(par->index,par->index))
    646646          << "   nEpo = " << par->numEpo;
    … …  
    651651          << setw(7) << setprecision(3) << aprTrp << " "
    652652          << setw(6) << setprecision(3) << showpos << par->xx << noshowpos
    653           << " +- " << setw(6) << setprecision(3) 
     653          << " +- " << setw(6) << setprecision(3)
    654654          << sqrt(_QQ(par->index,par->index));
    655655    }
    656656    else if (par->type == t_pppParam::GLONASS_OFFSET) {
    657       LOG << "\n    offGlo  = " << setw(10) << setprecision(3) << par->xx 
    658           << " +- " << setw(6) << setprecision(3) 
     657      LOG << "\n    offGlo  = " << setw(10) << setprecision(3) << par->xx
     658          << " +- " << setw(6) << setprecision(3)
    659659          << sqrt(_QQ(par->index,par->index));
    660660    }
    661661    else if (par->type == t_pppParam::GALILEO_OFFSET) {
    662       LOG << "\n    offGal  = " << setw(10) << setprecision(3) << par->xx 
    663           << " +- " << setw(6) << setprecision(3) 
     662      LOG << "\n    offGal  = " << setw(10) << setprecision(3) << par->xx
     663          << " +- " << setw(6) << setprecision(3)
    664664          << sqrt(_QQ(par->index,par->index));
    665665    }
    … …  
    679679  // Final Message (both log file and screen)
    680680  // ----------------------------------------
    681   LOG << OPT->_roverName << "  PPP " 
     681  LOG << OPT->_roverName << "  PPP "
    682682      << epoData->tt.timestr(1) << " " << epoData->sizeAll() << " "
    683683      << setw(14) << setprecision(3) << x()                  << " +- "
    … …  
    722722  QString prnGPS;
    723723  QString prnGlo;
    724   double  maxResGPS = 0.0; // all the other systems except GLONASS
    725   double  maxResGlo = 0.0; // GLONASS
     724  double  maxResGPS = 0.0; // GPS + Galileo
     725  double  maxResGlo = 0.0; // GLONASS + BDS
    726726  findMaxRes(vv, satData, prnGPS, prnGlo, maxResGPS, maxResGlo);
    727727
    728728  if      (iPhase == 1) {
    729     if      (maxResGlo > 2.98 * OPT->_maxResL1) { 
     729    if      (maxResGlo > 2.98 * OPT->_maxResL1) {
    730730      LOG << "Outlier Phase " << prnGlo.mid(0,3).toAscii().data() << ' ' << maxResGlo << endl;
    731731      return prnGlo;
    732732    }
    733     else if (maxResGPS > 2.98 * OPT->_maxResL1) { 
     733    else if (maxResGPS > 2.98 * OPT->_maxResL1) {
    734734      LOG << "Outlier Phase " << prnGPS.mid(0,3).toAscii().data() << ' ' << maxResGPS << endl;
    735735      return prnGPS;
    … …  
    762762  // -----------------------------------
    763763  if (!_windUpTime.contains(prn) || _windUpTime[prn] != Mjd) {
    764     _windUpTime[prn] = Mjd; 
     764    _windUpTime[prn] = Mjd;
    765765
    766766    // Unit Vector GPS Satellite --> Receiver
    … …  
    768768    ColumnVector rho = rRec - rSat;
    769769    rho /= rho.norm_Frobenius();
    770    
     770
    771771    // GPS Satellite unit Vectors sz, sy, sx
    772772    // -------------------------------------
    … …  
    781781    // Effective Dipole of the GPS Satellite Antenna
    782782    // ---------------------------------------------
    783     ColumnVector dipSat = sx - rho * DotProduct(rho,sx) 
     783    ColumnVector dipSat = sx - rho * DotProduct(rho,sx)
    784784                                                - crossproduct(rho, sy);
    785    
     785
    786786    // Receiver unit Vectors rx, ry
    787787    // ----------------------------
    … …  
    791791    double recEll[3]; xyz2ell(rRec.data(), recEll) ;
    792792    double neu[3];
    793    
     793
    794794    neu[0] = 1.0;
    795795    neu[1] = 0.0;
    796796    neu[2] = 0.0;
    797797    neu2xyz(recEll, neu, rx.data());
    798    
     798
    799799    neu[0] =  0.0;
    800800    neu[1] = -1.0;
    801801    neu[2] =  0.0;
    802802    neu2xyz(recEll, neu, ry.data());
    803    
     803
    804804    // Effective Dipole of the Receiver Antenna
    805805    // ----------------------------------------
    806     ColumnVector dipRec = rx - rho * DotProduct(rho,rx) 
     806    ColumnVector dipRec = rx - rho * DotProduct(rho,rx)
    807807                                                   + crossproduct(rho, ry);
    808    
     808
    809809    // Resulting Effect
    810810    // ----------------
    811     double alpha = DotProduct(dipSat,dipRec) / 
     811    double alpha = DotProduct(dipSat,dipRec) /
    812812                      (dipSat.norm_Frobenius() * dipRec.norm_Frobenius());
    813    
     813
    814814    if (alpha >  1.0) alpha =  1.0;
    815815    if (alpha < -1.0) alpha = -1.0;
    816    
     816
    817817    double dphi = acos(alpha) / 2.0 / M_PI;  // in cycles
    818    
     818
    819819    if ( DotProduct(rho, crossproduct(dipSat, dipRec)) < 0.0 ) {
    820820      dphi = -dphi;
    … …  
    824824  }
    825825
    826   return _windUpSum[prn]; 
    827 }
    828 
    829 // 
     826  return _windUpSum[prn];
     827}
     828
     829//
    830830///////////////////////////////////////////////////////////////////////////
    831831void t_pppFilter::cmpEle(t_satData* satData) {
    … …  
    844844}
    845845
    846 // 
     846//
    847847///////////////////////////////////////////////////////////////////////////
    848848void t_pppFilter::addAmb(t_satData* satData) {
    … …  
    853853  bool    found = false;
    854854  for (int iPar = 1; iPar <= _params.size(); iPar++) {
    855     if (_params[iPar-1]->type == t_pppParam::AMB_L3 && 
     855    if (_params[iPar-1]->type == t_pppParam::AMB_L3 &&
    856856        _params[iPar-1]->prn == satData->prn) {
    857857      found = true;
    … …  
    860860  }
    861861  if (!found) {
    862     t_pppParam* par = new t_pppParam(t_pppParam::AMB_L3, 
     862    t_pppParam* par = new t_pppParam(t_pppParam::AMB_L3,
    863863                                 _params.size()+1, satData->prn);
    864864    _params.push_back(par);
    … …  
    867867}
    868868
    869 // 
     869//
    870870///////////////////////////////////////////////////////////////////////////
    871871void t_pppFilter::addObs(int iPhase, unsigned& iObs, t_satData* satData,
    … …  
    876876  const double ELEWGHT = 20.0;
    877877  double ellWgtCoef = 1.0;
    878   double eleD = satData->eleSat * 180.0 / M_PI; 
     878  double eleD = satData->eleSat * 180.0 / M_PI;
    879879  if (eleD < ELEWGHT) {
    880880    ellWgtCoef = 1.5 - 0.5 / (ELEWGHT - 10.0) * (eleD - 10.0);
    … …  
    903903          _params[iPar-1]->prn  == satData->prn) {
    904904        ll(iObs) -= _params[iPar-1]->xx;
    905       } 
     905      }
    906906      AA(iObs, iPar) = _params[iPar-1]->partial(satData, true);
    907907    }
    … …  
    912912  else {
    913913    double sigP3 = 2.98 * OPT->_sigmaC1;
    914     if  (satData->system() == 'C') {
    915       sigP3 *= BDS_WEIGHT_FACTOR;
    916     }
    917914    ll(iObs)      = satData->P3 - cmpValue(satData, false);
    918915    PP(iObs,iObs) = 1.0 / (sigP3 * sigP3) / (ellWgtCoef * ellWgtCoef);
    … …  
    923920}
    924921
    925 // 
     922//
    926923///////////////////////////////////////////////////////////////////////////
    927 QByteArray t_pppFilter::printRes(int iPhase, const ColumnVector& vv, 
     924QByteArray t_pppFilter::printRes(int iPhase, const ColumnVector& vv,
    928925                              const QMap<QString, t_satData*>& satDataMap) {
    929926
    … …  
    950947}
    951948
    952 // 
     949//
    953950///////////////////////////////////////////////////////////////////////////
    954951void t_pppFilter::findMaxRes(const ColumnVector& vv,
    955952                          const QMap<QString, t_satData*>& satData,
    956                           QString& prnGPS, QString& prnGlo, 
    957                           double& maxResGPS, double& maxResGlo) { 
     953                          QString& prnGPS, QString& prnGlo,
     954                          double& maxResGPS, double& maxResGlo) {
    958955
    959956  Tracer tracer("t_pppFilter::findMaxRes");
    … …  
    968965    if (satData->obsIndex != 0) {
    969966      QString prn = satData->prn;
    970       if (prn[0] == 'R') {
     967      if (prn[0] == 'R' || prn[0] == 'C') {
    971968        if (fabs(vv(satData->obsIndex)) > maxResGlo) {
    972969          maxResGlo = fabs(vv(satData->obsIndex));
    … …  
    983980  }
    984981}
    985  
     982
    986983// Update Step (private - loop over outliers)
    987984////////////////////////////////////////////////////////////////////////////
    … …  
    10101007
    10111008    // First update using code observations, then phase observations
    1012     // -------------------------------------------------------------     
     1009    // -------------------------------------------------------------
    10131010    bool usePhase = OPT->ambLCs('G').size() || OPT->ambLCs('R').size() ||
    10141011                    OPT->ambLCs('E').size() || OPT->ambLCs('C').size() ;
    … …  
    10191016      // -----------------
    10201017      predict(iPhase, epoData);
    1021      
     1018
    10221019      // Create First-Design Matrix
    10231020      // --------------------------
    … …  
    10341031        }
    10351032      }
    1036      
     1033
    10371034      // Prepare first-design Matrix, vector observed-computed
    10381035      // -----------------------------------------------------
    10391036      Matrix          AA(nObs, nPar);  // first design matrix
    1040       ColumnVector    ll(nObs);        // tems observed-computed
     1037      ColumnVector    ll(nObs);        // terms observed-computed
    10411038      DiagonalMatrix  PP(nObs); PP = 0.0;
    1042      
     1039
    10431040      unsigned iObs = 0;
    10441041      QMapIterator<QString, t_satData*> it(epoData->satData);
    … …  
    10611058      kalman(AA, ll, PP, _QQ, dx);
    10621059      ColumnVector vv = ll - AA * dx;
    1063      
     1060
    10641061      // Print Residuals
    10651062      // ---------------
    1066       if      (iPhase == 0) {
     1063      if (iPhase == 0) {
    10671064        strResCode  = printRes(iPhase, vv, epoData->satData);
    10681065      }
    … …  
    11621159}
    11631160
    1164 // 
    1165 ////////////////////////////////////////////////////////////////////////////
    1166 t_irc t_pppFilter::selectSatellites(const QString& lastOutlierPrn, 
     1161//
     1162////////////////////////////////////////////////////////////////////////////
     1163t_irc t_pppFilter::selectSatellites(const QString& lastOutlierPrn,
    11671164                                 QMap<QString, t_satData*>& satData) {
    11681165
    1169   // First Call 
     1166  // First Call
    11701167  // ----------
    11711168  if (lastOutlierPrn.isEmpty()) {
    … …  
    11791176  else {
    11801177
    1181     if (lastOutlierPrn[0] == 'R') {
     1178    if (lastOutlierPrn[0] == 'R' || lastOutlierPrn[0] == 'C') {
    11821179      _outlierGlo << lastOutlierPrn;
    11831180    }
    … …  
    11931190    }
    11941191
    1195     if (lastOutlierPrn[0] == 'R') {
     1192    if (lastOutlierPrn[0] == 'R' || lastOutlierPrn[0] == 'C') {
    11961193      _outlierGPS.clear();
    11971194      return success;
    … …  
    12131210}
    12141211
    1215 // 
     1212//
    12161213////////////////////////////////////////////////////////////////////////////
    12171214double lorentz(const ColumnVector& aa, const ColumnVector& bb) {
    … …  
    12191216}
    12201217
    1221 // 
     1218//
    12221219////////////////////////////////////////////////////////////////////////////
    12231220void t_pppFilter::bancroft(const Matrix& BBpass, ColumnVector& pos) {
    … …  
    12371234      if (iter > 1) {
    12381235        double zz  = BB(ii,3);
    1239         double rho = sqrt( (xx-pos(1)) * (xx-pos(1)) + 
    1240                            (yy-pos(2)) * (yy-pos(2)) + 
     1236        double rho = sqrt( (xx-pos(1)) * (xx-pos(1)) +
     1237                           (yy-pos(2)) * (yy-pos(2)) +
    12411238                           (zz-pos(3)) * (zz-pos(3)) );
    12421239        traveltime = rho / t_CST::c;
    … …  
    12481245      BB(ii,2) = -sina * xx + cosa * yy;
    12491246    }
    1250    
     1247
    12511248    Matrix BBB;
    12521249    if (mm > 4) {
    … …  
    12601257    ColumnVector alpha(mm); alpha = 0.0;
    12611258    for (int ii = 1; ii <= mm; ii++) {
    1262       alpha(ii) = lorentz(BB.Row(ii).t(),BB.Row(ii).t())/2.0; 
     1259      alpha(ii) = lorentz(BB.Row(ii).t(),BB.Row(ii).t())/2.0;
    12631260    }
    12641261    ColumnVector BBBe     = BBB * ee;
    … …  
    12691266    double root = sqrt(bb*bb-aa*cc);
    12701267
    1271     Matrix hlpPos(4,2); 
     1268    Matrix hlpPos(4,2);
    12721269    hlpPos.Column(1) = (-bb-root)/aa * BBBe + BBBalpha;
    12731270    hlpPos.Column(2) = (-bb+root)/aa * BBBe + BBBalpha;
    … …  
    12761273    for (int pp = 1; pp <= 2; pp++) {
    12771274      hlpPos(4,pp)      = -hlpPos(4,pp);
    1278       omc(pp) = BB(1,4) - 
     1275      omc(pp) = BB(1,4) -
    12791276                sqrt( (BB(1,1)-hlpPos(1,pp)) * (BB(1,1)-hlpPos(1,pp)) +
    12801277                      (BB(1,2)-hlpPos(2,pp)) * (BB(1,2)-hlpPos(2,pp)) +
    1281                       (BB(1,3)-hlpPos(3,pp)) * (BB(1,3)-hlpPos(3,pp)) ) - 
     1278                      (BB(1,3)-hlpPos(3,pp)) * (BB(1,3)-hlpPos(3,pp)) ) -
    12821279                hlpPos(4,pp);
    12831280    }
    … …  
    12911288}
    12921289
    1293 // 
     1290//
    12941291////////////////////////////////////////////////////////////////////////////
    12951292void t_pppFilter::cmpDOP(t_epoData* epoData) {
    … …  
    13191316  }
    13201317  AA = AA.Rows(1, _numSat);
    1321   SymmetricMatrix NN; NN << AA.t() * AA; 
     1318  SymmetricMatrix NN; NN << AA.t() * AA;
    13221319  SymmetricMatrix QQ = NN.i();
    1323    
     1320
    13241321  _pDop = sqrt(QQ(1,1) + QQ(2,2) + QQ(3,3));
    13251322}
Note: See TracChangeset for help on using the changeset viewer.