Changeset 2073 in ntrip


Ignore:
Timestamp:
Dec 5, 2009, 12:07:52 PM (15 years ago)
Author:
mervart
Message:

* empty log message *

Location:
trunk/BNC
Files:
3 edited

Legend:

Unmodified
Added
Removed
  • trunk/BNC/bncmodel.cpp

    r2071 r2073  
    5252const unsigned MINOBS =    4;
    5353const double   MINELE = 10.0 * M_PI / 180.0;
     54const double   sig_crd_0 =  100.0;
     55const double   sig_crd_p =  100.0;
     56const double   sig_clk_0 = 1000.0;
    5457
    5558// Constructor
    5659////////////////////////////////////////////////////////////////////////////
    57 bncParam::bncParam(bncParam::parType typeIn) {
    58   type = typeIn;
     60bncParam::bncParam(bncParam::parType typeIn, int indexIn) {
     61  type  = typeIn;
     62  index = indexIn;
     63  x0    = 0.0;
     64  xx    = 0.0;
    5965}
    6066
     
    8692bncModel::bncModel() {
    8793  _xcBanc.ReSize(4); _xcBanc = 0.0;
    88   _params.push_back(new bncParam(bncParam::CRD_X));
    89   _params.push_back(new bncParam(bncParam::CRD_Y));
    90   _params.push_back(new bncParam(bncParam::CRD_Z));
    91   _params.push_back(new bncParam(bncParam::RECCLK));
     94  _params.push_back(new bncParam(bncParam::CRD_X,  1));
     95  _params.push_back(new bncParam(bncParam::CRD_Y,  2));
     96  _params.push_back(new bncParam(bncParam::CRD_Z,  3));
     97  _params.push_back(new bncParam(bncParam::RECCLK, 4));
    9298  _ellBanc.ReSize(3);
     99
     100  unsigned nPar = _params.size();
     101  _QQ.ReSize(nPar);
     102  _QQ = 0.0;
     103
     104  _QQ(1,1) = sig_crd_0 * sig_crd_0;
     105  _QQ(2,2) = sig_crd_0 * sig_crd_0;
     106  _QQ(3,3) = sig_crd_0 * sig_crd_0;
     107  _QQ(4,4) = sig_clk_0 * sig_clk_0;
     108
     109  _xx.ReSize(nPar);
     110  _xx = 0.0;
    93111}
    94112
     
    123141  bancroft(BB, _xcBanc);
    124142
    125   // Set Parameter A Priori Values
    126   // -----------------------------
    127   QListIterator<bncParam*> itPar(_params);
    128   while (itPar.hasNext()) {
    129     bncParam* par = itPar.next();
    130     if      (par->type == bncParam::CRD_X) {
    131       par->x0 = _xcBanc(1);
    132     }
    133     else if (par->type == bncParam::CRD_Y) {
    134       par->x0 = _xcBanc(2);
    135     }
    136     else if (par->type == bncParam::CRD_Z) {
    137       par->x0 = _xcBanc(3);
    138     }
    139     else if (par->type == bncParam::RECCLK) {
    140       par->x0 = _xcBanc(4);
    141     }
    142   }
    143 
    144143  // Ellipsoidal Coordinates
    145144  // ------------------------
     
    179178double bncModel::cmpValueP3(t_satData* satData) {
    180179
    181   double rho0 = (satData->xx - _xcBanc.Rows(1,3)).norm_Frobenius();
    182 
    183180  ColumnVector xRec(3);
     181  xRec(1) = x();
     182  xRec(2) = y();
     183  xRec(3) = z();
     184
     185  double rho0 = (satData->xx - xRec).norm_Frobenius();
    184186  double dPhi = t_CST::omega * rho0 / t_CST::c;
    185   xRec(1) = _xcBanc(1) * cos(dPhi) - _xcBanc(2) * sin(dPhi);
    186   xRec(2) = _xcBanc(2) * cos(dPhi) + _xcBanc(1) * sin(dPhi);
    187   xRec(3) = _xcBanc(3);
     187
     188  xRec(1) = x() * cos(dPhi) - y() * sin(dPhi);
     189  xRec(2) = y() * cos(dPhi) + x() * sin(dPhi);
     190  xRec(3) = z();
    188191
    189192  satData->rho = (satData->xx - xRec).norm_Frobenius();
     
    191194  double tropDelay = delay_saast(satData->eleSat);
    192195
    193   return satData->rho + _xcBanc(4) - satData->clk + tropDelay;
     196  return satData->rho + clk() - satData->clk + tropDelay;
    194197}
    195198
     
    227230}
    228231
     232// Prediction Step of the Filter
     233////////////////////////////////////////////////////////////////////////////
     234void bncModel::predict() {
     235
     236  _params[0]->x0 = _xcBanc(1);
     237  _params[1]->x0 = _xcBanc(2);
     238  _params[2]->x0 = _xcBanc(3);
     239  _params[3]->x0 = _xcBanc(4);
     240
     241  _params[0]->xx = 0.0;
     242  _params[1]->xx = 0.0;
     243  _params[2]->xx = 0.0;
     244  _params[3]->xx = 0.0;
     245
     246  _QQ(1,1) += sig_crd_p * sig_crd_p;
     247  _QQ(2,2) += sig_crd_p * sig_crd_p;
     248  _QQ(3,3) += sig_crd_p * sig_crd_p;
     249
     250  for (int iPar = 1; iPar <= _params.size(); iPar++) {
     251    _QQ(iPar, 4) = 0.0;
     252  }
     253  _QQ(4,4) = sig_clk_0 * sig_clk_0;
     254
     255  _xx = 0.0;
     256}
     257
    229258// Update Step of the Filter (currently just a single-epoch solution)
    230259////////////////////////////////////////////////////////////////////////////
     
    235264  }
    236265
     266  predict();
     267
    237268  unsigned nPar = _params.size();
    238269  unsigned nObs = epoData->size();
     
    240271  // Create First-Design Matrix
    241272  // --------------------------
    242   _AA.ReSize(nObs, nPar);  // variance-covariance matrix
    243   _ll.ReSize(nObs);        // tems observed-computed
     273  Matrix       AA(nObs, nPar);  // first design matrix
     274  ColumnVector ll(nObs);        // tems observed-computed
    244275
    245276  unsigned iObs = 0;
     
    250281    QString    prn     = itObs.key();
    251282    t_satData* satData = itObs.value();
    252     _ll(iObs) = satData->P3 - cmpValueP3(satData);
    253 
    254     unsigned iPar = 0;
    255     QListIterator<bncParam*> itPar(_params);
    256     while (itPar.hasNext()) {
    257       ++iPar;
    258       bncParam* par = itPar.next();
    259       _AA(iObs, iPar) = par->partialP3(satData);
     283    ll(iObs) = satData->P3 - cmpValueP3(satData);
     284
     285    for (int iPar = 1; iPar <= _params.size(); iPar++) {
     286      AA(iObs, iPar) = _params[iPar-1]->partialP3(satData);
    260287    }
    261288  }
    262289
    263   // Compute Least-Squares Solution
    264   // ------------------------------
    265   _QQ.ReSize(nPar);
    266   _QQ << _AA.t() * _AA;
    267   _QQ = _QQ.i();
    268   _dx = _QQ * _AA.t() * _ll;
    269 
    270   // Compute Residuals
    271   // -----------------
    272   ColumnVector vv = _AA * _dx - _ll;
     290  // Compute Kalman Update
     291  // ---------------------
     292  IdentityMatrix  PP(nObs);
     293  SymmetricMatrix HH; HH << PP + AA * _QQ * AA.t();
     294  SymmetricMatrix Hi = HH.i();
     295  Matrix          KK  = _QQ * AA.t() * Hi;
     296  ColumnVector    v1  = ll - AA * _xx;
     297                  _xx = _xx + KK * v1;
     298  IdentityMatrix Id(nPar);
     299  _QQ << (Id - KK * AA) * _QQ;
    273300
    274301  // Set Solution Vector
    275302  // -------------------
    276   _xx.ReSize(nPar);
    277   unsigned iPar = 0;
    278   QListIterator<bncParam*> itPar(_params);
     303  QVectorIterator<bncParam*> itPar(_params);
    279304  while (itPar.hasNext()) {
    280     ++iPar;
    281305    bncParam* par = itPar.next();
    282     _xx(iPar) = par->x0 + _dx(iPar);
     306    par->xx = _xx(par->index);
    283307  }
    284308
  • trunk/BNC/bncmodel.h

    r2065 r2073  
    3737 public:
    3838  enum parType {CRD_X, CRD_Y, CRD_Z, RECCLK, TROPO, AMB_L3};
    39   bncParam(parType typeIn);
     39  bncParam(parType typeIn, int indexIn);
    4040  ~bncParam();
    4141  double partialP3(t_satData* satData);
     42  bool isCrd() const {
     43    return (type == CRD_X || type == CRD_Y || type == CRD_Z);
     44  }
     45  double solVal() const {return x0 + xx;}
     46  double aprVal() const {return x0;}
    4247  parType  type;
     48  double   xx;
    4349  double   x0;
     50  int      index;
    4451};
    4552
     
    5057  t_irc cmpBancroft(t_epoData* epoData);
    5158  t_irc update(t_epoData* epoData);
    52   const ColumnVector& xcBanc() const {return _xcBanc;}
    53   const ColumnVector& xx()     const {return _xx;}
     59  double x()   const {return _params[0]->solVal();}
     60  double y()   const {return _params[1]->solVal();}
     61  double z()   const {return _params[2]->solVal();}
     62  double clk() const {return _params[3]->solVal();}
    5463 
    5564 private:
    5665  double cmpValueP3(t_satData* satData);
    5766  double delay_saast(double Ele);
     67  void   predict();
    5868
    59   QList<bncParam*> _params;
    60   SymmetricMatrix  _QQ;
    61   Matrix           _AA;
    62   ColumnVector     _ll;
    63   ColumnVector     _dx;
    64   ColumnVector     _xx;
    65   ColumnVector     _xcBanc;
    66   ColumnVector     _ellBanc;
     69  QVector<bncParam*> _params;
     70  SymmetricMatrix    _QQ;
     71  ColumnVector       _xx;
     72  ColumnVector       _xcBanc;
     73  ColumnVector       _ellBanc;
    6774};
    6875
  • trunk/BNC/bncpppclient.cpp

    r2069 r2073  
    344344  str << "    PPP " << _staID.data() << " "
    345345      << _epoData->tt.timestr(1) << " " << _epoData->size() << " "
    346       << setw(14) << setprecision(3) << _model->xx()(1) << "  "
    347       << setw(14) << setprecision(3) << _model->xx()(2) << "  "
    348       << setw(14) << setprecision(3) << _model->xx()(3);
     346      << setw(14) << setprecision(3) << _model->x() << "  "
     347      << setw(14) << setprecision(3) << _model->y() << "  "
     348      << setw(14) << setprecision(3) << _model->z();
    349349
    350350  emit newMessage(QString(str.str().c_str()).toAscii(), true);
Note: See TracChangeset for help on using the changeset viewer.