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

Last change on this file since 6430 was 6430, checked in by mervart, 9 years ago
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
51bool excludeSat(const t_prn& prn) {
52 if (prn == t_prn('R', 7)) {
53 return true;
54 }
55 else {
56 return false;
57 }
58}
59
60// Constructor
61////////////////////////////////////////////////////////////////////////////
62t_sp3Comp::t_sp3Comp(QObject* parent) : QThread(parent) {
63
64 bncSettings settings;
65 _sp3FileNames = settings.value("sp3CompFile").toString().split(',', QString::SkipEmptyParts);
66 for (int ii = 0; ii < _sp3FileNames.size(); ii++) {
67 expandEnvVar(_sp3FileNames[ii]);
68 }
69 _logFileName = settings.value("sp3CompOutLogFile").toString(); expandEnvVar(_logFileName);
70 _logFile = 0;
71 _log = 0;
72
73 //// beg test
74 _excludeSats.insert(t_prn('R', 2));
75 _excludeSats.insert(t_prn('R', 3));
76 _excludeSats.insert(t_prn('R', 4));
77 _excludeSats.insert(t_prn('R', 5));
78 _excludeSats.insert(t_prn('R', 6));
79 _excludeSats.insert(t_prn('R', 7));
80 _excludeSats.insert(t_prn('R', 9));
81 _excludeSats.insert(t_prn('R', 10));
82 _excludeSats.insert(t_prn('R', 11));
83 _excludeSats.insert(t_prn('R', 12));
84 _excludeSats.insert(t_prn('R', 13));
85 _excludeSats.insert(t_prn('R', 14));
86 _excludeSats.insert(t_prn('R', 15));
87 _excludeSats.insert(t_prn('R', 16));
88 _excludeSats.insert(t_prn('R', 17));
89 _excludeSats.insert(t_prn('R', 18));
90 _excludeSats.insert(t_prn('R', 19));
91 _excludeSats.insert(t_prn('R', 20));
92 _excludeSats.insert(t_prn('R', 21));
93 _excludeSats.insert(t_prn('R', 22));
94 _excludeSats.insert(t_prn('R', 23));
95 _excludeSats.insert(t_prn('R', 24));
96 //// end test
97}
98
99// Destructor
100////////////////////////////////////////////////////////////////////////////
101t_sp3Comp::~t_sp3Comp() {
102 delete _log;
103 delete _logFile;
104}
105
106//
107////////////////////////////////////////////////////////////////////////////
108void t_sp3Comp::run() {
109
110 // Open Log File
111 // -------------
112 _logFile = new QFile(_logFileName);
113 if (_logFile->open(QIODevice::WriteOnly | QIODevice::Text)) {
114 _log = new QTextStream();
115 _log->setDevice(_logFile);
116 }
117 if (!_log) {
118 goto end;
119 }
120
121 for (int ii = 0; ii < _sp3FileNames.size(); ii++) {
122 *_log << "! SP3 File " << ii+1 << ": " << _sp3FileNames[ii] << endl;
123 }
124 if (_sp3FileNames.size() != 2) {
125 *_log << "ERROR: sp3Comp requires two input SP3 files" << endl;
126 goto end;
127 }
128
129 try {
130 ostringstream msg;
131 compare(msg);
132 *_log << msg.str().c_str();
133 }
134 catch (const string& error) {
135 *_log << "ERROR: " << error.c_str() << endl;
136 }
137 catch (const char* error) {
138 *_log << "ERROR: " << error << endl;
139 }
140 catch (Exception& exc) {
141 *_log << "ERROR: " << exc.what() << endl;
142 }
143 catch (...) {
144 *_log << "ERROR: " << "unknown exception" << endl;
145 }
146
147 // Exit (thread)
148 // -------------
149 end:
150 if (BNC_CORE->mode() != t_bncCore::interactive) {
151 qApp->exit(0);
152 }
153 else {
154 emit finished();
155 deleteLater();
156 }
157}
158
159// Satellite Index in clkSats set
160////////////////////////////////////////////////////////////////////////////////
161int t_sp3Comp::satIndex(const set<t_prn>& clkSats, const t_prn& prn) const {
162 int ret = 0;
163 for (set<t_prn>::const_iterator it = clkSats.begin(); it != clkSats.end(); it++) {
164 if ( *it == prn) {
165 return ret;
166 }
167 ++ret;
168 }
169 return -1;
170}
171
172// Estimate Clock Offsets
173////////////////////////////////////////////////////////////////////////////////
174void t_sp3Comp::processClocks(const set<t_prn>& clkSats, const vector<t_epoch*>& epochsIn,
175 map<string, t_stat>& stat) const {
176
177 if (clkSats.size() == 0) {
178 return;
179 }
180
181 vector<t_epoch*> epochs;
182 for (unsigned ii = 0; ii < epochsIn.size(); ii++) {
183 if (epochsIn[ii]->_dc.size() > 0) {
184 epochs.push_back(epochsIn[ii]);
185 }
186 }
187 if (epochs.size() == 0) {
188 return;
189 }
190
191 int nPar = epochs.size() + clkSats.size();
192 SymmetricMatrix NN(nPar); NN = 0.0;
193 ColumnVector bb(nPar); bb = 0.0;
194
195 // Create Matrix A'A and vector b
196 // ------------------------------
197 for (unsigned ie = 0; ie < epochs.size(); ie++) {
198 const map<t_prn, double>& dc = epochs[ie]->_dc;
199 Matrix AA(dc.size(), nPar); AA = 0.0;
200 ColumnVector ll(dc.size()); ll = 0.0;
201 map<t_prn, double>::const_iterator it;
202 int ii = -1;
203 for (it = dc.begin(); it != dc.end(); it++) {
204 const t_prn& prn = it->first;
205 if (satIndex(clkSats, prn) != -1) {
206 ++ii;
207 int index = epochs.size() + satIndex(clkSats, prn);
208 AA[ii][ie] = 1.0; // epoch-specfic offset (common for all satellites)
209 AA[ii][index] = 1.0; // satellite-specific offset (common for all epochs)
210 ll[ii] = it->second;
211 }
212 }
213 SymmetricMatrix dN; dN << AA.t() * AA;
214 NN += dN;
215 bb += AA.t() * ll;
216 }
217
218 // Regularize NN
219 // -------------
220 RowVector HH(nPar);
221 HH.columns(1, epochs.size()) = 0.0;
222 HH.columns(epochs.size()+1, nPar) = 1.0;
223 SymmetricMatrix dN; dN << HH.t() * HH;
224 NN += dN;
225
226 // Estimate Parameters
227 // -------------------
228 ColumnVector xx = NN.i() * bb;
229
230 // Compute clock residuals
231 // -----------------------
232 for (unsigned ie = 0; ie < epochs.size(); ie++) {
233 map<t_prn, double>& dc = epochs[ie]->_dc;
234 for (map<t_prn, double>::iterator it = dc.begin(); it != dc.end(); it++) {
235 const t_prn& prn = it->first;
236 if (satIndex(clkSats, prn) != -1) {
237 int index = epochs.size() + satIndex(clkSats, prn);
238 dc[prn] = it->second - xx[ie] - xx[index];
239 stat[prn.toString()]._offset = xx[index];
240 }
241 }
242 }
243}
244
245// Main Routine
246////////////////////////////////////////////////////////////////////////////////
247void t_sp3Comp::compare(ostringstream& out) const {
248
249 // Synchronize reading of two sp3 files
250 // ------------------------------------
251 bncSP3 in1(_sp3FileNames[0]); in1.nextEpoch();
252 bncSP3 in2(_sp3FileNames[1]); in2.nextEpoch();
253
254 vector<t_epoch*> epochs;
255 while (in1.currEpoch() && in2.currEpoch()) {
256 bncTime t1 = in1.currEpoch()->_tt;
257 bncTime t2 = in2.currEpoch()->_tt;
258 if (t1 < t2) {
259 in1.nextEpoch();
260 }
261 else if (t1 > t2) {
262 in2.nextEpoch();
263 }
264 else if (t1 == t2) {
265 t_epoch* epo = new t_epoch; epo->_tt = t1;
266 bool epochOK = false;
267 for (int i1 = 0; i1 < in1.currEpoch()->_sp3Sat.size(); i1++) {
268 bncSP3::t_sp3Sat* sat1 = in1.currEpoch()->_sp3Sat[i1];
269 for (int i2 = 0; i2 < in2.currEpoch()->_sp3Sat.size(); i2++) {
270 bncSP3::t_sp3Sat* sat2 = in2.currEpoch()->_sp3Sat[i2];
271 if (sat1->_prn == sat2->_prn) {
272 epochOK = true;
273 epo->_dr[sat1->_prn] = sat1->_xyz - sat2->_xyz;
274 epo->_xyz[sat1->_prn] = sat1->_xyz;
275 if (sat1->_clkValid && sat2->_clkValid) {
276 epo->_dc[sat1->_prn] = sat1->_clk - sat2->_clk;
277 }
278 }
279 }
280 }
281 if (epochOK) {
282 epochs.push_back(epo);
283 }
284 else {
285 delete epo;
286 }
287 in1.nextEpoch();
288 in2.nextEpoch();
289 }
290 }
291
292 // Transform xyz into radial, along-track, and out-of-plane
293 // --------------------------------------------------------
294 if (epochs.size() < 2) {
295 throw "t_sp3Comp: not enough common epochs";
296 }
297
298 set<t_prn> clkSatsAll;
299
300 for (unsigned ie = 0; ie < epochs.size(); ie++) {
301 t_epoch* epoch = epochs[ie];
302 t_epoch* epoch2 = 0;
303 if (ie == 0) {
304 epoch2 = epochs[ie+1];
305 }
306 else {
307 epoch2 = epochs[ie-1];
308 }
309 double dt = epoch->_tt - epoch2->_tt;
310 map<t_prn, ColumnVector>& dr = epoch->_dr;
311 map<t_prn, ColumnVector>& xyz = epoch->_xyz;
312 map<t_prn, ColumnVector>& xyz2 = epoch2->_xyz;
313 for (map<t_prn, ColumnVector>::const_iterator it = dr.begin(); it != dr.end(); it++) {
314 const t_prn& prn = it->first;
315 if (xyz2.find(prn) != xyz2.end()) {
316 const ColumnVector dx = dr[prn];
317 const ColumnVector& x1 = xyz[prn];
318 const ColumnVector& x2 = xyz2[prn];
319 ColumnVector vel = (x1 - x2) / dt;
320 XYZ_to_RSW(x1, vel, dx, dr[prn]);
321 if (epoch->_dc.find(prn) != epoch->_dc.end()) {
322 clkSatsAll.insert(prn);
323 }
324 }
325 else {
326 epoch->_dr.erase(prn);
327 epoch->_xyz.erase(prn);
328 epoch->_dc.erase(prn);
329 }
330 }
331 }
332
333 map<string, t_stat> stat;
334
335 // Estimate Clock Offsets
336 // ----------------------
337 string systems;
338 for (set<t_prn>::const_iterator it = clkSatsAll.begin(); it != clkSatsAll.end(); it++) {
339 if (systems.find(it->system()) == string::npos) {
340 systems += it->system();
341 }
342 }
343 for (unsigned iSys = 0; iSys < systems.size(); iSys++) {
344 char system = systems[iSys];
345 set<t_prn> clkSats;
346 for (set<t_prn>::const_iterator it = clkSatsAll.begin(); it != clkSatsAll.end(); it++) {
347 if (it->system() == system && !excludeSat(*it)) {
348 clkSats.insert(*it);
349 }
350 }
351 processClocks(clkSats, epochs, stat);
352 }
353
354 // Print Residuals
355 // ---------------
356 const string all = "ZZZ";
357
358 out.setf(ios::fixed);
359 out << "!\n! MJD PRN radial along out clk clkRed iPRN"
360 "\n! ----------------------------------------------------------------\n";
361 for (unsigned ii = 0; ii < epochs.size(); ii++) {
362 const t_epoch* epo = epochs[ii];
363 const map<t_prn, ColumnVector>& dr = epochs[ii]->_dr;
364 const map<t_prn, double>& dc = epochs[ii]->_dc;
365 for (map<t_prn, ColumnVector>::const_iterator it = dr.begin(); it != dr.end(); it++) {
366 const t_prn& prn = it->first;
367 if (!excludeSat(prn)) {
368 const ColumnVector& rao = it->second;
369 out << setprecision(6) << epo->_tt.mjddec() << ' ' << prn.toString() << ' '
370 << setw(7) << setprecision(4) << rao[0] << ' '
371 << setw(7) << setprecision(4) << rao[1] << ' '
372 << setw(7) << setprecision(4) << rao[2] << " ";
373 stat[prn.toString()]._rao += SP(rao, rao); // Schur product
374 stat[prn.toString()]._nr += 1;
375 stat[all]._rao += SP(rao, rao);
376 stat[all]._nr += 1;
377 if (dc.find(prn) != dc.end()) {
378 double clkRes = dc.find(prn)->second;
379 double clkResRed = clkRes - it->second[0]; // clock minus radial component
380 out << setw(7) << setprecision(4) << clkRes << ' '
381 << setw(7) << setprecision(4) << clkResRed;
382 stat[prn.toString()]._dc += clkRes * clkRes;
383 stat[prn.toString()]._dcRed += clkResRed * clkResRed;
384 stat[prn.toString()]._nc += 1;
385 stat[all]._dc += clkRes * clkRes;
386 stat[all]._dcRed += clkResRed * clkResRed;
387 stat[all]._nc += 1;
388 }
389 else {
390 out << " . . ";
391 }
392 out << " " << setw(2) << int(prn) << endl;
393 }
394 }
395 delete epo;
396 }
397
398 // Print Summary
399 // -------------
400 out << "!\n! RMS:\n";
401 for (map<string, t_stat>::iterator it = stat.begin(); it != stat.end(); it++) {
402 const string& prn = it->first;
403 t_stat& stat = it->second;
404 if (stat._nr > 0) {
405 stat._rao[0] = sqrt(stat._rao[0] / stat._nr);
406 stat._rao[1] = sqrt(stat._rao[1] / stat._nr);
407 stat._rao[2] = sqrt(stat._rao[2] / stat._nr);
408 if (prn == all) {
409 out << "!\n! Total ";
410 }
411 else {
412 out << "! " << prn << ' ';
413 }
414 out << setw(7) << setprecision(4) << stat._rao[0] << ' '
415 << setw(7) << setprecision(4) << stat._rao[1] << ' '
416 << setw(7) << setprecision(4) << stat._rao[2] << " ";
417 if (stat._nc > 0) {
418 stat._dc = sqrt(stat._dc / stat._nc);
419 stat._dcRed = sqrt(stat._dcRed / stat._nc);
420 out << setw(7) << setprecision(4) << stat._dc << ' '
421 << setw(7) << setprecision(4) << stat._dcRed;
422 if (prn != all) {
423 out << " offset " << setw(9) << setprecision(4) << stat._offset;
424 }
425 }
426 else {
427 out << " . . ";
428 }
429 out << endl;
430 }
431 }
432}
433
434//
435////////////////////////////////////////////////////////////////////////////
436bool t_sp3Comp::excludeSat(const t_prn& prn) const {
437 if (_excludeSats.find(prn) != _excludeSats.end()) {
438 return true;
439 }
440 else {
441 return false;
442 }
443}
444
Note: See TracBrowser for help on using the repository browser.