Changeset 6345 in ntrip

Timestamp:

Nov 26, 2014, 5:37:10 PM (9 years ago)

Author:

mervart

Message:

 

Location:

trunk/BNC/src/orbComp

Files:

• sp3Comp.cpp (modified) (6 diffs)
• sp3Comp.h (modified) (2 diffs)

trunk/BNC/src/orbComp/sp3Comp.cpp

@@ -105 +105 @@
 }
 
-//// Satellite Index in clkSats set
-//////////////////////////////////////////////////////////////////////////////////
-//int satIndex(const set<GPSS::t_prn>& clkSats, const GPSS::t_prn& prn) {
-//  int ret = 0;
-//  for (set<GPSS::t_prn>::const_iterator it = clkSats.begin(); it != clkSats.end(); it++) {
-//    if ( *it == prn) {
-//      return ret;
-//    }
-//    ++ret;
-//  }
-//  cerr << "satellite not found in used Sats " << prn << endl;
-//  exit(1);
-//}
-//
-//// Estimate Clock Offsets
-//////////////////////////////////////////////////////////////////////////////////
-//void processClocks(const set<GPSS::t_prn>& clkSats, vector<t_epoch*>& epochs,
-//                   map<string, t_stat>& stat) {
-//
-//  if (clkSats.size() == 0) {
-//    return;
-//  }
-//
-//  int nPar = epochs.size() + clkSats.size();
-//  SymmetricMatrix NN(nPar); NN = 0.0;
-//  ColumnVector    bb(nPar); bb = 0.0;
-//
-//  // Create Matrix A'A and vector b
-//  // ------------------------------
-//  for (unsigned ie = 0; ie < epochs.size(); ie++) {
-//    const GPSS::t_gpstime&          tt = epochs[ie]->_tt;
-//    const map<GPSS::t_prn, double>& dc = epochs[ie]->_dc;
-//    Matrix       AA(dc.size(), nPar); AA = 0.0;
-//    ColumnVector ll(dc.size());       ll = 0.0;
-//    map<GPSS::t_prn, double>::const_iterator it; int ii;
-//    for (it = dc.begin(), ii = 0; it != dc.end(); it++, ii++) {
-//      const GPSS::t_prn& prn = it->first;
-//      int index = epochs.size() + satIndex(clkSats, prn);
-//      AA[ii][ie]    = 1.0; // epoch-specfic offset (common for all satellites)
-//      AA[ii][index] = 1.0; // satellite-specific offset (common for all epochs)
-//      ll[ii]        = it->second;
-//    }
-//    SymmetricMatrix dN; dN << AA.t() * AA;
-//    NN += dN;
-//    bb += AA.t() * ll;
-//  }
-//
-//  // Regularize NN
-//  // -------------
-//  RowVector HH(nPar); 
-//  HH.columns(1, epochs.size())      = 0.0;
-//  HH.columns(epochs.size()+1, nPar) = 1.0;
-//  SymmetricMatrix dN; dN << HH.t() * HH;
-//  NN += dN;
-// 
-//  // Estimate Parameters
-//  // -------------------
-//  ColumnVector xx = NN.i() * bb;
-//
-//  // Compute clock residuals
-//  // -----------------------
-//  for (unsigned ie = 0; ie < epochs.size(); ie++) {
-//    const GPSS::t_gpstime&  tt     = epochs[ie]->_tt;
-//    map<GPSS::t_prn, double>& dc = epochs[ie]->_dc;
-//    for (map<GPSS::t_prn, double>::iterator it = dc.begin(); it != dc.end(); it++) {
-//      const GPSS::t_prn& prn = it->first;
-//      int  index = epochs.size() + satIndex(clkSats, prn);
-//      dc[prn]           = it->second - xx[ie] - xx[index];
-//      stat[prn]._offset = xx[index];
-//    }
-//  }
-//}
-//
+// Satellite Index in clkSats set
+////////////////////////////////////////////////////////////////////////////////
+int t_sp3Comp::satIndex(const set<t_prn>& clkSats, const t_prn& prn) const {
+  int ret = 0;
+  for (set<t_prn>::const_iterator it = clkSats.begin(); it != clkSats.end(); it++) {
+    if ( *it == prn) {
+      return ret;
+    }
+    ++ret;
+  }
+  throw "satellite not found " + prn.toString();
+}
+
+// Estimate Clock Offsets
+////////////////////////////////////////////////////////////////////////////////
+void t_sp3Comp::processClocks(const set<t_prn>& clkSats, vector<t_epoch*>& epochs,
+                              map<string, t_stat>& stat) const {
+
+  if (clkSats.size() == 0) {
+    return;
+  }
+
+  int nPar = epochs.size() + clkSats.size();
+  SymmetricMatrix NN(nPar); NN = 0.0;
+  ColumnVector    bb(nPar); bb = 0.0;
+
+  // Create Matrix A'A and vector b
+  // ------------------------------
+  for (unsigned ie = 0; ie < epochs.size(); ie++) {
+    ///    const bncTime&          tt = epochs[ie]->_tt;
+    const map<t_prn, double>& dc = epochs[ie]->_dc;
+    Matrix       AA(dc.size(), nPar); AA = 0.0;
+    ColumnVector ll(dc.size());       ll = 0.0;
+    map<t_prn, double>::const_iterator it; int ii;
+    for (it = dc.begin(), ii = 0; it != dc.end(); it++, ii++) {
+      const t_prn& prn = it->first;
+      int index = epochs.size() + satIndex(clkSats, prn);
+      AA[ii][ie]    = 1.0; // epoch-specfic offset (common for all satellites)
+      AA[ii][index] = 1.0; // satellite-specific offset (common for all epochs)
+      ll[ii]        = it->second;
+    }
+    SymmetricMatrix dN; dN << AA.t() * AA;
+    NN += dN;
+    bb += AA.t() * ll;
+  }
+
+  // Regularize NN
+  // -------------
+  RowVector HH(nPar); 
+  HH.columns(1, epochs.size())      = 0.0;
+  HH.columns(epochs.size()+1, nPar) = 1.0;
+  SymmetricMatrix dN; dN << HH.t() * HH;
+  NN += dN;
+ 
+  // Estimate Parameters
+  // -------------------
+  ColumnVector xx = NN.i() * bb;
+
+  // Compute clock residuals
+  // -----------------------
+  for (unsigned ie = 0; ie < epochs.size(); ie++) {
+    // const bncTime&  tt     = epochs[ie]->_tt;
+    map<t_prn, double>& dc = epochs[ie]->_dc;
+    for (map<t_prn, double>::iterator it = dc.begin(); it != dc.end(); it++) {
+      const t_prn& prn = it->first;
+      int  index = epochs.size() + satIndex(clkSats, prn);
+      dc[prn]                      = it->second - xx[ie] - xx[index];
+      stat[prn.toString()]._offset = xx[index];
+    }
+  }
+}
+
 //// Program
 //////////////////////////////////////////////////////////////////////////////////
 //int main(int argc, char* argv[]) {
 //
-//  GPSS::t_sysout* sysout = new GPSS::t_sysout("", 0, false);
-//  GPSS::rtnet_core_log  = sysout->getSysoutCore();
+//  t_sysout* sysout = new t_sysout("", 0, false);
+//  rtnet_core_log  = sysout->getSysoutCore();
 //
 //  // Parse Input Options
@@ -196 +195 @@
 //  // Synchronize reading of two sp3 files
 //  // ------------------------------------
-//  vector<GPSS::t_prn> prnList;
-//  for (unsigned prn = 1; prn <= GPSS::t_prn::MAXPRN; prn++) {
-//    prnList.push_back(GPSS::t_prn(prn));
-//  }
-//  GPSS::t_sp3 in1(prnList); if (in1.assignFile(OPT["sp3File1"].c_str())) return 1;
-//  GPSS::t_sp3 in2(prnList); if (in2.assignFile(OPT["sp3File2"].c_str())) return 1;
+//  vector<t_prn> prnList;
+//  for (unsigned prn = 1; prn <= t_prn::MAXPRN; prn++) {
+//    prnList.push_back(t_prn(prn));
+//  }
+//  t_sp3 in1(prnList); if (in1.assignFile(OPT["sp3File1"].c_str())) return 1;
+//  t_sp3 in2(prnList); if (in2.assignFile(OPT["sp3File2"].c_str())) return 1;
 //
 //  vector<t_epoch*> epochs;
-//  set<GPSS::t_prn> clkSats;
+//  set<t_prn> clkSats;
 //  while (true) {
 //    in1.readNextEpoch();
-//    GPSS::t_gpstime tt = in1.etime();
+//    bncTime tt = in1.etime();
 //    if (!tt.valid()) {
 //      break;
@@ -214 +213 @@
 //      t_epoch* epo = new t_epoch; epo->_tt = tt;
 //      bool epochOK = false;
-//      for (unsigned iPrn = 1; iPrn <= GPSS::t_prn::MAXPRN; iPrn++) {
-//        GPSS::t_prn prn(iPrn);
+//      for (unsigned iPrn = 1; iPrn <= t_prn::MAXPRN; iPrn++) {
+//        t_prn prn(iPrn);
 //        ColumnVector xyz1(3), xyz2(3);
 //        ColumnVector vel1(3), vel2(3);
@@ -261 +260 @@
 //    }
 //    double dt = epoch->_tt - epoch2->_tt;
-//    map<GPSS::t_prn, ColumnVector>& dr   = epoch->_dr;
-//    map<GPSS::t_prn, ColumnVector>& xyz  = epoch->_xyz;
-//    map<GPSS::t_prn, ColumnVector>& xyz2 = epoch2->_xyz;
-//    for (map<GPSS::t_prn, ColumnVector>::const_iterator it = dr.begin(); it != dr.end(); it++) {
-//      const GPSS::t_prn&  prn = it->first;
+//    map<t_prn, ColumnVector>& dr   = epoch->_dr;
+//    map<t_prn, ColumnVector>& xyz  = epoch->_xyz;
+//    map<t_prn, ColumnVector>& xyz2 = epoch2->_xyz;
+//    for (map<t_prn, ColumnVector>::const_iterator it = dr.begin(); it != dr.end(); it++) {
+//      const t_prn&  prn = it->first;
 //      if (xyz2.find(prn) != xyz2.end()) {
 //        const ColumnVector  dx = dr[prn];
@@ -271 +270 @@
 //        const ColumnVector& x2 = xyz2[prn];
 //        ColumnVector vel = (x1 - x2) / dt;
-//        GPSS::t_astro::XYZ_to_RSW(x1, vel, dx, dr[prn]);
+//        t_astro::XYZ_to_RSW(x1, vel, dx, dr[prn]);
 //      }
 //      else {
@@ -294 +293 @@
 //  for (unsigned ii = 0; ii < epochs.size(); ii++) {
 //    const t_epoch* epo = epochs[ii];
-//    const map<GPSS::t_prn, ColumnVector>& dr = epochs[ii]->_dr;
-//    const map<GPSS::t_prn, double>&       dc = epochs[ii]->_dc;
-//    for (map<GPSS::t_prn, ColumnVector>::const_iterator it = dr.begin(); it != dr.end(); it++) {
-//      const GPSS::t_prn&  prn = it->first;
+//    const map<t_prn, ColumnVector>& dr = epochs[ii]->_dr;
+//    const map<t_prn, double>&       dc = epochs[ii]->_dc;
+//    for (map<t_prn, ColumnVector>::const_iterator it = dr.begin(); it != dr.end(); it++) {
+//      const t_prn&  prn = it->first;
 //      const ColumnVector& rao = it->second;
 //      cout << setprecision(6) << epo->_tt.mjddec() << ' ' << prn << ' '

trunk/BNC/src/orbComp/sp3Comp.h

@@ -27 +27 @@
 
 #include <map>
+#include <set>
+#include <vector>
+#include <string>
 #include <QtCore>
 #include <newmat.h>
@@ -77 +80 @@
   };
 
+  int  satIndex(const std::set<t_prn>& clkSats, const t_prn& prn) const;
+  void processClocks(const std::set<t_prn>& clkSats, std::vector<t_epoch*>& epochs,
+                     std::map<std::string, t_stat>& stat) const;
+
   QStringList  _sp3FileNames;
   QString      _logFileName;