Changeset 6344 in ntrip

Timestamp:

Nov 26, 2014, 5:28:23 PM (9 years ago)

Author:

mervart

Message:

 

Location:

trunk/BNC/src/orbComp

Files:

• sp3Comp.cpp (modified) (1 diff)
• 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];
+//    }
+//  }
+//}
+//
+//// Program
+//////////////////////////////////////////////////////////////////////////////////
+//int main(int argc, char* argv[]) {
+//
+//  GPSS::t_sysout* sysout = new GPSS::t_sysout("", 0, false);
+//  GPSS::rtnet_core_log  = sysout->getSysoutCore();
+//
+//  // Parse Input Options
+//  // -------------------
+//  map<string, string> OPT;
+//  parseCmdLine(argc, argv, OPT);
+//  if (OPT.find("sp3File1") == OPT.end() || OPT.find("sp3File2") == OPT.end()) {
+//    cerr << "usage: sp3comp --sp3File1 <fileName> --sp3File2 <fileName> " << endl;
+//    return 1;
+//  }
+//
+//  // 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_epoch*> epochs;
+//  set<GPSS::t_prn> clkSats;
+//  while (true) {
+//    in1.readNextEpoch();
+//    GPSS::t_gpstime tt = in1.etime();
+//    if (!tt.valid()) {
+//      break;
+//    }
+//    if (in2.readEpoch(tt) == t_irc::success) {
+//      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);
+//        ColumnVector xyz1(3), xyz2(3);
+//        ColumnVector vel1(3), vel2(3);
+//        double       clk1, clk2;
+//        bool         posSet1, posSet2;
+//        bool         clkSet1, clkSet2;
+//        bool         velSet1, velSet2;
+//        if (in1.position(prn, xyz1[0], xyz1[1], xyz1[2], posSet1, clk1, clkSet1,
+//                         vel1[0], vel1[1], vel1[2], velSet1) == 0 &&
+//            in2.position(prn, xyz2[0], xyz2[1], xyz2[2], posSet2, clk2, clkSet2,
+//                         vel1[0], vel1[1], vel1[2], velSet1) == 0) {
+//          if (posSet1 && posSet2) {
+//            epochOK       = true;
+//            epo->_dr[prn]  = xyz1 - xyz2;
+//            epo->_xyz[prn] = xyz1;
+//            if (clkSet1 && clkSet2) {
+//              epo->_dc[prn] = (clk1 - clk2) * t_genConst::c;
+//              clkSats.insert(prn);
+//            }
+//          }
+//        }
+//      }
+//      if (epochOK) {
+//        epochs.push_back(epo);
+//      }
+//      else {
+//        delete epo;
+//      }
+//    }
+//    if (in1.finished()) {
+//      break;
+//    }
+//  }
+//
+//  // Transform xyz into radial, along-track, and out-of-plane
+//  // --------------------------------------------------------
+//  for (unsigned ie = 0; ie < epochs.size(); ie++) {
+//    t_epoch* epoch  = epochs[ie];
+//    t_epoch* epoch2 = 0;
+//    if (ie == 0 && epochs.size() > 1) {
+//      epoch2 = epochs[ie+1];
+//    }
+//    else {
+//      epoch2 = epochs[ie-1];
+//    }
+//    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;
+//      if (xyz2.find(prn) != xyz2.end()) {
+//        const ColumnVector  dx = dr[prn];
+//        const ColumnVector& x1 = xyz[prn];
+//        const ColumnVector& x2 = xyz2[prn];
+//        ColumnVector vel = (x1 - x2) / dt;
+//        GPSS::t_astro::XYZ_to_RSW(x1, vel, dx, dr[prn]);
+//      }
+//      else {
+//        cerr << "not found: " << prn << endl;
+//      }
+//    }
+//  }
+//
+//  map<string, t_stat> stat;
+//
+//  // Estimate Clock Offsets
+//  // ----------------------
+//  processClocks(clkSats, epochs, stat);
+//
+//  // Print Residuals
+//  // ---------------
+//  const string all = "ZZZ";
+//
+//  cout.setf(ios::fixed);
+//  cout << "!\n!  MJD       PRN  radial   along   out        clk    clkRed   iPRN"
+//           "\n! ----------------------------------------------------------------\n";
+//  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 ColumnVector& rao = it->second;
+//      cout << setprecision(6) << epo->_tt.mjddec() << ' ' << prn << ' '
+//           << setw(7) << setprecision(4) << rao[0] << ' '
+//           << setw(7) << setprecision(4) << rao[1] << ' '
+//           << setw(7) << setprecision(4) << rao[2] << "    ";
+//      stat[prn]._rao += SP(rao, rao); // Schur product
+//      stat[prn]._nr  += 1;
+//      stat[all]._rao += SP(rao, rao);
+//      stat[all]._nr  += 1;
+//      if (dc.find(prn) != dc.end()) {
+//        double clkRes    = dc.find(prn)->second;
+//        double clkResRed = clkRes - it->second[0]; // clock minus radial component
+//        cout << setw(7) << setprecision(4) << clkRes << ' '
+//             << setw(7) << setprecision(4) << clkResRed;
+//        stat[prn]._dc    += clkRes * clkRes;
+//        stat[prn]._dcRed += clkResRed * clkResRed;
+//        stat[prn]._nc    += 1;
+//        stat[all]._dc    += clkRes * clkRes;
+//        stat[all]._dcRed += clkResRed * clkResRed;
+//        stat[all]._nc    += 1;
+//      }
+//      else {
+//        cout << "  .       .    ";
+//      }
+//      cout << "    " << setw(2) << int(prn) << endl;
+//    }
+//    delete epo;
+//  }
+//
+//  // Print Summary
+//  // -------------
+//  cout << "!\n! RMS:\n";
+//  for (map<string, t_stat>::iterator it = stat.begin(); it != stat.end(); it++) {
+//    const string& prn  = it->first;
+//    t_stat&       stat = it->second;
+//    if (stat._nr > 0) {
+//      stat._rao[0] = sqrt(stat._rao[0] / stat._nr);
+//      stat._rao[1] = sqrt(stat._rao[1] / stat._nr);
+//      stat._rao[2] = sqrt(stat._rao[2] / stat._nr);
+//      if (prn == all) {
+//        cout << "!\n!          Total ";
+//      }
+//      else {
+//        cout << "!            " << prn << ' ';
+//      }
+//      cout << setw(7) << setprecision(4) << stat._rao[0] << ' '
+//           << setw(7) << setprecision(4) << stat._rao[1] << ' '
+//           << setw(7) << setprecision(4) << stat._rao[2] << "    ";
+//      if (stat._nc > 0) {
+//        stat._dc    = sqrt(stat._dc / stat._nc);
+//        stat._dcRed = sqrt(stat._dcRed / stat._nc);
+//        cout << setw(7) << setprecision(4) << stat._dc << ' '
+//             << setw(7) << setprecision(4) << stat._dcRed;
+//        if (prn != all) {
+//          cout << "    offset " << setw(7) << setprecision(4) << stat._offset;
+//        }
+//      }
+//      else {
+//        cout << "  .       .    ";
+//      }
+//      cout << endl;
+//    }
+//  }
+//
+//  return 0;
+//}

trunk/BNC/src/orbComp/sp3Comp.h

@@ -26 +26 @@
 #define SP3COMP_H
 
+#include <map>
 #include <QtCore>
+#include <newmat.h>
+#include "bnctime.h"
+#include "t_prn.h"
 
 class t_sp3Comp : public QThread {
@@ -46 +50 @@
  
  private:
+  class t_epoch {
+   public:
+    bncTime                  _tt;
+    std::map<t_prn, ColumnVector> _dr;
+    std::map<t_prn, ColumnVector> _xyz;
+    std::map<t_prn, double>       _dc;
+  };
+
+  class t_stat {
+   public:
+    t_stat() {
+      _rao.ReSize(3); 
+      _rao    = 0.0;
+      _dc     = 0.0;
+      _dcRed  = 0.0;
+      _offset = 0.0;
+      _nr     = 0;
+      _nc     = 0;
+    }
+    ColumnVector _rao;
+    double       _dc;
+    double       _dcRed;
+    double       _offset;
+    int          _nr;
+    int          _nc;
+  };
+
   QStringList  _sp3FileNames;
   QString      _logFileName;