Changeset 2080 in ntrip

Timestamp:

Dec 5, 2009, 3:01:36 PM (14 years ago)

Author:

mervart

Message:

* empty log message *

Location:

trunk/BNC

Files:

• bncmodel.cpp (modified) (9 diffs)
• bncmodel.h (modified) (3 diffs)

trunk/BNC/bncmodel.cpp

@@ -56 +56 @@
 const double   sig_crd_p =  100.0;
 const double   sig_clk_0 = 1000.0;
+const double   sig_amb_0 =  100.0;
 
 // Constructor
 ////////////////////////////////////////////////////////////////////////////
-bncParam::bncParam(bncParam::parType typeIn, int indexIn) {
-  type  = typeIn;
-  index = indexIn;
-  x0    = 0.0;
-  xx    = 0.0;
+bncParam::bncParam(bncParam::parType typeIn, int indexIn, 
+                   const QString& prnIn) {
+  type      = typeIn;
+  index     = indexIn;
+  prn       = prnIn;
+  index_old = 0;
+  x0        = 0.0;
+  xx        = 0.0;
 }
 
@@ -73 +77 @@
 // Partial
 ////////////////////////////////////////////////////////////////////////////
-double bncParam::partialP3(t_satData* satData) {
+double bncParam::partial(t_satData* satData, const QString& prnIn) {
   if      (type == CRD_X) {
     return (x0 - satData->xx(1)) / satData->rho; 
@@ -86 +90 @@
     return 1.0;
   }
+  else if (type == AMB_L3) {
+    if (prnIn == prn) {
+      return 1.0;
+    }
+    else {
+      return 0.0;
+    }
+  }
   return 0.0;
 }
@@ -93 +105 @@
 bncModel::bncModel() {
   _xcBanc.ReSize(4); _xcBanc = 0.0;
-  _params.push_back(new bncParam(bncParam::CRD_X,  1));
-  _params.push_back(new bncParam(bncParam::CRD_Y,  2));
-  _params.push_back(new bncParam(bncParam::CRD_Z,  3));
-  _params.push_back(new bncParam(bncParam::RECCLK, 4));
+  _params.push_back(new bncParam(bncParam::CRD_X,  1, ""));
+  _params.push_back(new bncParam(bncParam::CRD_Y,  2, ""));
+  _params.push_back(new bncParam(bncParam::CRD_Z,  3, ""));
+  _params.push_back(new bncParam(bncParam::RECCLK, 4, ""));
   _ellBanc.ReSize(3);
 
@@ -184 +196 @@
 // Computed Value
 ////////////////////////////////////////////////////////////////////////////
-double bncModel::cmpValueP3(t_satData* satData) {
+double bncModel::cmpValue(t_satData* satData) {
 
   ColumnVector xRec(3);
@@ -240 +252 @@
 // Prediction Step of the Filter
 ////////////////////////////////////////////////////////////////////////////
-void bncModel::predict() {
+void bncModel::predict(t_epoData* epoData) {
+
+  // Make a copy of QQ and xx, set parameter indices
+  // -----------------------------------------------
+  SymmetricMatrix QQ_old = _QQ;
+  ColumnVector    xx_old = _xx;
+
+  for (int iPar = 1; iPar <= _params.size(); iPar++) {
+    _params[iPar-1]->index_old = _params[iPar-1]->index;
+    _params[iPar-1]->index     = 0;
+  }
+
+  // Remove Ambiguity Parameters without observations
+  // ------------------------------------------------
+  int iPar = 0;
+  QMutableVectorIterator<bncParam*> it(_params);
+  while (it.hasNext()) {
+    bncParam* par = it.next();
+    bool removed = false;
+    if (par->type == bncParam::AMB_L3) {
+      if (epoData->satData.find(par->prn) == epoData->satData.end()) {
+        removed = true;
+        delete par;
+        it.remove();
+      }
+    }
+    if (! removed) {
+      ++iPar;
+      par->index = iPar;
+    }
+  }
+
+  // Add new ambiguity parameters
+  // ----------------------------
+  QMapIterator<QString, t_satData*> itObs(epoData->satData);
+  while (itObs.hasNext()) {
+    itObs.next();
+    QString    prn     = itObs.key();
+    bool found = false;
+    for (int iPar = 1; iPar <= _params.size(); iPar++) {
+      if (_params[iPar-1]->type == bncParam::AMB_L3 && 
+          _params[iPar-1]->prn == prn) {
+        found = true;
+        break;
+      }
+    }
+    if (!found) {
+      bncParam* par = new bncParam(bncParam::AMB_L3, _params.size()+1, prn);
+      _params.push_back(par);
+    }
+  }
+
+  int nPar = _params.size();
+  _xx.ReSize(nPar); _xx = 0.0;
+  _QQ.ReSize(nPar); _QQ = 0.0;
+  for (int i1 = 1; i1 <= nPar; i1++) {
+    bncParam* p1 = _params[i1-1];
+    if (p1->index_old != 0) {
+      _xx(p1->index)            = xx_old(p1->index_old);
+      _QQ(p1->index, p1->index) = QQ_old(p1->index_old, p1->index_old);
+      for (int i2 = 1; i2 <= nPar; i2++) {
+        bncParam* p2 = _params[i2-1];
+        if (p2->index_old != 0) {
+          _QQ(p1->index, p2->index) = QQ_old(p1->index_old, p2->index_old);
+        }
+      }
+    }
+  }
+
+  for (int ii = 1; ii <= nPar; ii++) {
+    bncParam* par = _params[ii-1];
+    if (par->index_old == 0) {
+      _QQ(par->index, par->index) = sig_amb_0 * sig_amb_0;
+    }
+    par->index_old = par->index;
+  }
 
   // Coordinates
@@ -274 +361 @@
   _QQ(4,4) = sig_clk_0 * sig_clk_0;
 
+  // Ambiguities
+  // -----------
+  for (int iPar = 1; iPar <= _params.size(); iPar++) {
+    if (_params[iPar-1]->type == bncParam::AMB_L3) {
+      _params[iPar-1]->x0 += _params[iPar-1]->xx;
+    }
+  }
+
   // Nullify the Solution Vector
   // ---------------------------
@@ -290 +385 @@
   }
 
-  predict();
+  predict(epoData);
 
   unsigned nPar = _params.size();
-  unsigned nObs = epoData->size();
+  unsigned nObs = 2 * epoData->size();
 
   // Create First-Design Matrix
@@ -307 +402 @@
     QString    prn     = itObs.key();
     t_satData* satData = itObs.value();
-    ll(iObs) = satData->P3 - cmpValueP3(satData);
-
+
+    double rhoCmp = cmpValue(satData);
+
+    ll(iObs) = satData->P3 - rhoCmp;
     for (int iPar = 1; iPar <= _params.size(); iPar++) {
-      AA(iObs, iPar) = _params[iPar-1]->partialP3(satData);
+      AA(iObs, iPar) = _params[iPar-1]->partial(satData, "");
+    }
+
+    ++iObs;
+    ll(iObs) = satData->L3 - rhoCmp;
+    for (int iPar = 1; iPar <= _params.size(); iPar++) {
+      if (_params[iPar-1]->type == bncParam::AMB_L3 &&
+          _params[iPar-1]->prn  == prn) {
+        ll(iObs) -= _params[iPar-1]->x0;
+      } 
+      AA(iObs, iPar) = _params[iPar-1]->partial(satData, prn);
     }
   }

trunk/BNC/bncmodel.h

@@ -37 +37 @@
  public:
   enum parType {CRD_X, CRD_Y, CRD_Z, RECCLK, TROPO, AMB_L3};
-  bncParam(parType typeIn, int indexIn);
+  bncParam(parType typeIn, int indexIn, const QString& prn);
   ~bncParam();
-  double partialP3(t_satData* satData);
+  double partial(t_satData* satData, const QString& prnIn);
   bool isCrd() const {
     return (type == CRD_X || type == CRD_Y || type == CRD_Z);
@@ -49 +49 @@
   double   x0;
   int      index;
+  int      index_old;
+  QString  prn;
 };
 
@@ -63 +65 @@
   
  private:
-  double cmpValueP3(t_satData* satData);
+  double cmpValue(t_satData* satData);
   double delay_saast(double Ele);
-  void   predict();
+  void   predict(t_epoData* epoData);
 
   QVector<bncParam*> _params;