source: ntrip/trunk/BNC/src/orbComp/sp3Comp.cpp @ 9175

Last change on this file since 9175 was 9175, checked in by stuerze, 3 months ago

minor changes

File size: 13.4 KB
Line 
1// Part of BNC, a utility for retrieving decoding and
2// converting GNSS data streams from NTRIP broadcasters.
3//
4// Copyright (C) 2007
5// German Federal Agency for Cartography and Geodesy (BKG)
6// http://www.bkg.bund.de
7// Czech Technical University Prague, Department of Geodesy
8// http://www.fsv.cvut.cz
9//
10// Email: euref-ip@bkg.bund.de
11//
12// This program is free software; you can redistribute it and/or
13// modify it under the terms of the GNU General Public License
14// as published by the Free Software Foundation, version 2.
15//
16// This program is distributed in the hope that it will be useful,
17// but WITHOUT ANY WARRANTY; without even the implied warranty of
18// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19// GNU General Public License for more details.
20//
21// You should have received a copy of the GNU General Public License
22// along with this program; if not, write to the Free Software
23// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
24
25/* -------------------------------------------------------------------------
26 * BKG NTRIP Client
27 * -------------------------------------------------------------------------
28 *
29 * Class:      t_sp3Comp
30 *
31 * Purpose:    Compare SP3 Files
32 *
33 * Author:     L. Mervart
34 *
35 * Created:    24-Nov-2014
36 *
37 * Changes:
38 *
39 * -----------------------------------------------------------------------*/
40
41#include <iostream>
42#include <iomanip>
43#include "sp3Comp.h"
44#include "bnccore.h"
45#include "bncsettings.h"
46#include "bncutils.h"
47#include "bncsp3.h"
48
49using namespace std;
50
51// Constructor
52////////////////////////////////////////////////////////////////////////////
53t_sp3Comp::t_sp3Comp(QObject* parent) : QThread(parent) {
54
55  bncSettings settings;
56  _sp3FileNames = settings.value("sp3CompFile").toString().split(QRegExp("[ ,]"), QString::SkipEmptyParts);
57  for (int ii = 0; ii < _sp3FileNames.size(); ii++) {
58    expandEnvVar(_sp3FileNames[ii]);
59  }
60  _logFileName = settings.value("sp3CompOutLogFile").toString(); expandEnvVar(_logFileName);
61  _logFile     = 0;
62  _log         = 0;
63
64  _excludeSats = settings.value("sp3CompExclude").toString().split(QRegExp("[ ,]"), QString::SkipEmptyParts);
65}
66
67// Destructor
68////////////////////////////////////////////////////////////////////////////
69t_sp3Comp::~t_sp3Comp() {
70  delete _log;
71  delete _logFile;
72}
73
74//
75////////////////////////////////////////////////////////////////////////////
76void t_sp3Comp::run() {
77
78  // Open Log File
79  // -------------
80  _logFile = new QFile(_logFileName);
81  if (_logFile->open(QIODevice::WriteOnly | QIODevice::Text)) {
82    _log = new QTextStream();
83    _log->setDevice(_logFile);
84  }
85  if (!_log) {
86    cerr << "ERROR: SP3Comp requires logfile specification" << endl;
87    goto exit;
88  }
89
90  for (int ii = 0; ii < _sp3FileNames.size(); ii++) {
91    *_log << "! SP3 File " << ii+1 << ": " << _sp3FileNames[ii] << endl;
92  }
93  if (_sp3FileNames.size() != 2) {
94    *_log << "ERROR: sp3Comp requires two input SP3 files" << endl;
95    goto end;
96  }
97
98  try {
99    ostringstream msg;
100    compare(msg);
101    *_log << msg.str().c_str();
102  }
103  catch (const string& error) {
104    *_log << "ERROR: " << error.c_str() << endl;
105  }
106  catch (const char* error) {
107    *_log << "ERROR: " << error << endl;
108  }
109  catch (Exception& exc) {
110    *_log << "ERROR: " << exc.what() << endl;
111  }
112  catch (std::exception& exc) {
113    *_log << "ERROR: " << exc.what() << endl;
114  }
115  catch (QString error) {
116    *_log << "ERROR: " << error << endl;
117  }
118  catch (...) {
119    *_log << "ERROR: " << "unknown exception" << endl;
120  }
121
122  // Exit (thread)
123  // -------------
124 end:
125  _log->flush();
126 exit:
127  // do nothing if no logfile available
128
129  if (BNC_CORE->mode() != t_bncCore::interactive) {
130    qApp->exit(0);
131    msleep(100); //sleep 0.1 sec
132  }
133  else {
134    emit finished();
135    deleteLater();
136  }
137}
138
139// Satellite Index in clkSats set
140////////////////////////////////////////////////////////////////////////////////
141int t_sp3Comp::satIndex(const set<t_prn>& clkSats, const t_prn& prn) const {
142  int ret = 0;
143  for (set<t_prn>::const_iterator it = clkSats.begin(); it != clkSats.end(); it++) {
144    if ( *it == prn) {
145      return ret;
146    }
147    ++ret;
148  }
149  return -1;
150}
151
152// Estimate Clock Offsets
153////////////////////////////////////////////////////////////////////////////////
154void t_sp3Comp::processClocks(const set<t_prn>& clkSats, const vector<t_epoch*>& epochsIn,
155                              map<string, t_stat>& stat) const {
156
157  if (clkSats.size() == 0) {
158    return;
159  }
160
161  vector<t_epoch*> epochs;
162  for (unsigned ii = 0; ii < epochsIn.size(); ii++) {
163    unsigned numSatOK = 0;
164    std::map<t_prn, double>::const_iterator it;
165    for (it = epochsIn[ii]->_dc.begin(); it != epochsIn[ii]->_dc.end(); it++) {
166      if (satIndex(clkSats, it->first) != -1) {
167        numSatOK += 1;
168      }
169    }
170    if (numSatOK > 0) {
171      epochs.push_back(epochsIn[ii]);
172    }
173  }
174  if (epochs.size() == 0) {
175    return;
176  }
177
178  int nPar = epochs.size() + clkSats.size();
179  SymmetricMatrix NN(nPar); NN = 0.0;
180  ColumnVector    bb(nPar); bb = 0.0;
181
182  // Create Matrix A'A and vector b
183  // ------------------------------
184  for (unsigned ie = 0; ie < epochs.size(); ie++) {
185    const map<t_prn, double>& dc = epochs[ie]->_dc;
186    Matrix       AA(dc.size(), nPar); AA = 0.0;
187    ColumnVector ll(dc.size());       ll = 0.0;
188    map<t_prn, double>::const_iterator it;
189    int ii = -1;
190    for (it = dc.begin(); it != dc.end(); it++) {
191      const t_prn& prn = it->first;
192      if (satIndex(clkSats, prn) != -1) {
193        ++ii;
194        int index = epochs.size() + satIndex(clkSats, prn);
195        AA[ii][ie]    = 1.0; // epoch-specfic offset (common for all satellites)
196        AA[ii][index] = 1.0; // satellite-specific offset (common for all epochs)
197        ll[ii]        = it->second;
198      }
199    }
200    SymmetricMatrix dN; dN << AA.t() * AA;
201    NN += dN;
202    bb += AA.t() * ll;
203  }
204
205  // Regularize NN
206  // -------------
207  RowVector HH(nPar);
208  HH.columns(1, epochs.size())      = 0.0;
209  HH.columns(epochs.size()+1, nPar) = 1.0;
210  SymmetricMatrix dN; dN << HH.t() * HH;
211  NN += dN;
212
213  // Estimate Parameters
214  // -------------------
215  ColumnVector xx = NN.i() * bb;
216
217  // Compute clock residuals
218  // -----------------------
219  for (unsigned ie = 0; ie < epochs.size(); ie++) {
220    map<t_prn, double>& dc = epochs[ie]->_dc;
221    for (map<t_prn, double>::iterator it = dc.begin(); it != dc.end(); it++) {
222      const t_prn& prn = it->first;
223      if (satIndex(clkSats, prn) != -1) {
224        int  index = epochs.size() + satIndex(clkSats, prn);
225        dc[prn]                      = it->second - xx[ie] - xx[index];
226        stat[prn.toString()]._offset = xx[index];
227      }
228    }
229  }
230}
231
232// Main Routine
233////////////////////////////////////////////////////////////////////////////////
234void t_sp3Comp::compare(ostringstream& out) const {
235
236  // Synchronize reading of two sp3 files
237  // ------------------------------------
238  bncSP3 in1(_sp3FileNames[0]); in1.nextEpoch();
239  bncSP3 in2(_sp3FileNames[1]); in2.nextEpoch();
240
241  vector<t_epoch*> epochs;
242  while (in1.currEpoch() && in2.currEpoch()) {
243    bncTime t1 = in1.currEpoch()->_tt;
244    bncTime t2 = in2.currEpoch()->_tt;
245    if      (t1 < t2) {
246      in1.nextEpoch();
247    }
248    else if (t1 > t2) {
249      in2.nextEpoch();
250    }
251    else if (t1 == t2) {
252      t_epoch* epo = new t_epoch; epo->_tt = t1;
253      bool epochOK = false;
254      for (int i1 = 0; i1 < in1.currEpoch()->_sp3Sat.size(); i1++) {
255        bncSP3::t_sp3Sat* sat1 = in1.currEpoch()->_sp3Sat[i1];
256        for (int i2 = 0; i2 < in2.currEpoch()->_sp3Sat.size(); i2++) {
257          bncSP3::t_sp3Sat* sat2 = in2.currEpoch()->_sp3Sat[i2];
258          if (sat1->_prn == sat2->_prn) {
259            epochOK        = true;
260            epo->_dr[sat1->_prn]  = sat1->_xyz - sat2->_xyz;
261            epo->_xyz[sat1->_prn] = sat1->_xyz;
262            if (sat1->_clkValid && sat2->_clkValid) {
263              epo->_dc[sat1->_prn] = sat1->_clk - sat2->_clk;
264            }
265          }
266        }
267      }
268      if (epochOK) {
269        epochs.push_back(epo);
270      }
271      else {
272        delete epo;
273      }
274      in1.nextEpoch();
275      in2.nextEpoch();
276    }
277  }
278
279  // Transform xyz into radial, along-track, and out-of-plane
280  // --------------------------------------------------------
281  if (epochs.size() < 2) {
282    throw "t_sp3Comp: not enough common epochs";
283  }
284
285  set<t_prn> clkSatsAll;
286
287  for (unsigned ie = 0; ie < epochs.size(); ie++) {
288    t_epoch* epoch  = epochs[ie];
289    t_epoch* epoch2 = 0;
290    if (ie == 0) {
291      epoch2 = epochs[ie+1];
292    }
293    else {
294      epoch2 = epochs[ie-1];
295    }
296    double dt = epoch->_tt - epoch2->_tt;
297    map<t_prn, ColumnVector>& dr   = epoch->_dr;
298    map<t_prn, ColumnVector>& xyz  = epoch->_xyz;
299    map<t_prn, ColumnVector>& xyz2 = epoch2->_xyz;
300    for (map<t_prn, ColumnVector>::const_iterator it = dr.begin(); it != dr.end(); it++) {
301      const t_prn&  prn = it->first;
302      if (xyz2.find(prn) != xyz2.end()) {
303        const ColumnVector  dx = dr[prn];
304        const ColumnVector& x1 = xyz[prn];
305        const ColumnVector& x2 = xyz2[prn];
306        ColumnVector vel = (x1 - x2) / dt;
307        XYZ_to_RSW(x1, vel, dx, dr[prn]);
308        if (epoch->_dc.find(prn) != epoch->_dc.end()) {
309          clkSatsAll.insert(prn);
310        }
311      }
312      else {
313        epoch->_dc.erase(prn);
314        epoch->_dr.erase(prn);
315      }
316    }
317  }
318
319  map<string, t_stat> stat;
320
321  // Estimate Clock Offsets
322  // ----------------------
323  string systems;
324  for (set<t_prn>::const_iterator it = clkSatsAll.begin(); it != clkSatsAll.end(); it++) {
325    if (systems.find(it->system()) == string::npos) {
326      systems += it->system();
327    }
328  }
329  for (unsigned iSys = 0; iSys < systems.size(); iSys++) {
330    char system = systems[iSys];
331    set<t_prn> clkSats;
332    for (set<t_prn>::const_iterator it = clkSatsAll.begin(); it != clkSatsAll.end(); it++) {
333      if (it->system() == system && !excludeSat(*it)) {
334        clkSats.insert(*it);
335      }
336    }
337    processClocks(clkSats, epochs, stat);
338  }
339
340  // Print Residuals
341  // ---------------
342  const string all = "ZZZ";
343
344  out.setf(ios::fixed);
345  out << "!\n!  MJD       PRN  radial   along   out        clk    clkRed   iPRN"
346           "\n! ----------------------------------------------------------------\n";
347  for (unsigned ii = 0; ii < epochs.size(); ii++) {
348    const t_epoch* epo = epochs[ii];
349    const map<t_prn, ColumnVector>& dr = epochs[ii]->_dr;
350    const map<t_prn, double>&       dc = epochs[ii]->_dc;
351    for (map<t_prn, ColumnVector>::const_iterator it = dr.begin(); it != dr.end(); it++) {
352      const t_prn&  prn = it->first;
353      if (!excludeSat(prn)) {
354        const ColumnVector& rao = it->second;
355        out << setprecision(6) << epo->_tt.mjddec() << ' ' << prn.toString() << ' '
356            << setw(7) << setprecision(4) << rao[0] << ' '
357            << setw(7) << setprecision(4) << rao[1] << ' '
358            << setw(7) << setprecision(4) << rao[2] << "    ";
359        stat[prn.toString()]._rao += SP(rao, rao); // Schur product
360        stat[prn.toString()]._nr  += 1;
361        stat[all]._rao            += SP(rao, rao);
362        stat[all]._nr             += 1;
363        if (dc.find(prn) != dc.end()) {
364          double clkRes    = dc.find(prn)->second;
365          double clkResRed = clkRes - it->second[0]; // clock minus radial component
366          out << setw(7) << setprecision(4) << clkRes << ' '
367              << setw(7) << setprecision(4) << clkResRed;
368          stat[prn.toString()]._dc    += clkRes * clkRes;
369          stat[prn.toString()]._dcRed += clkResRed * clkResRed;
370          stat[prn.toString()]._nc    += 1;
371          stat[all]._dc               += clkRes * clkRes;
372          stat[all]._dcRed            += clkResRed * clkResRed;
373          stat[all]._nc               += 1;
374        }
375        else {
376          out << "  .       .    ";
377        }
378        out << "    " << setw(2) << int(prn) << endl;
379      }
380    }
381    delete epo;
382  }
383
384  // Print Summary
385  // -------------
386  out << "!\n! RMS[m]\n";
387  out << "!\n!    PRN  radial   along   out     nOrb    clk   clkRed   nClk    Offset"
388           "\n! ----------------------------------------------------------------------\n";
389  for (map<string, t_stat>::iterator it = stat.begin(); it != stat.end(); it++) {
390    const string& prn  = it->first;
391    t_stat&       stat = it->second;
392    if (stat._nr > 0) {
393      stat._rao[0] = sqrt(stat._rao[0] / stat._nr);
394      stat._rao[1] = sqrt(stat._rao[1] / stat._nr);
395      stat._rao[2] = sqrt(stat._rao[2] / stat._nr);
396      if (prn == all) {
397        out << "!\n!  Total ";
398      }
399      else {
400        out << "!    " << prn << ' ';
401      }
402      out << setw(7) << setprecision(4) << stat._rao[0] << ' '
403          << setw(7) << setprecision(4) << stat._rao[1] << ' '
404          << setw(7) << setprecision(4) << stat._rao[2] << ' '
405          << setw(6) << stat._nr << " ";
406      if (stat._nc > 0) {
407        stat._dc    = sqrt(stat._dc / stat._nc);
408        stat._dcRed = sqrt(stat._dcRed / stat._nc);
409        out << setw(7) << setprecision(4) << stat._dc << ' '
410            << setw(7) << setprecision(4) << stat._dcRed << ' '
411            << setw(6) << stat._nc << " ";
412        if (prn != all) {
413          out << setw(9) << setprecision(4) << stat._offset;
414        }
415      }
416      else {
417        out << "  .       .    ";
418      }
419      out << endl;
420    }
421  }
422}
423
424//
425////////////////////////////////////////////////////////////////////////////
426bool t_sp3Comp::excludeSat(const t_prn& prn) const {
427  QStringListIterator it(_excludeSats);
428  while (it.hasNext()) {
429    string prnStr = it.next().toLatin1().data();
430    if (prnStr == prn.toString() || prnStr == prn.toString().substr(0,1)) {
431      return true;
432    }
433  }
434  return false;
435}
436
Note: See TracBrowser for help on using the repository browser.