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

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