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

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