[6333] | 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>
|
---|
[6348] | 42 | #include <iomanip>
|
---|
[6333] | 43 | #include "sp3Comp.h"
|
---|
| 44 | #include "bnccore.h"
|
---|
| 45 | #include "bncsettings.h"
|
---|
| 46 | #include "bncutils.h"
|
---|
[6348] | 47 | #include "bncsp3.h"
|
---|
[6333] | 48 |
|
---|
| 49 | using namespace std;
|
---|
| 50 |
|
---|
| 51 | // Constructor
|
---|
| 52 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 53 | t_sp3Comp::t_sp3Comp(QObject* parent) : QThread(parent) {
|
---|
| 54 |
|
---|
[6338] | 55 | bncSettings settings;
|
---|
[6341] | 56 | _sp3FileNames = settings.value("sp3CompFile").toString().split(',', QString::SkipEmptyParts);
|
---|
[6338] | 57 | for (int ii = 0; ii < _sp3FileNames.size(); ii++) {
|
---|
| 58 | expandEnvVar(_sp3FileNames[ii]);
|
---|
| 59 | }
|
---|
[6339] | 60 | _logFileName = settings.value("sp3CompOutLogFile").toString(); expandEnvVar(_logFileName);
|
---|
| 61 | _logFile = 0;
|
---|
| 62 | _log = 0;
|
---|
[6333] | 63 | }
|
---|
| 64 |
|
---|
| 65 | // Destructor
|
---|
| 66 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 67 | t_sp3Comp::~t_sp3Comp() {
|
---|
[6339] | 68 | delete _log;
|
---|
| 69 | delete _logFile;
|
---|
[6333] | 70 | }
|
---|
| 71 |
|
---|
| 72 | //
|
---|
| 73 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 74 | void t_sp3Comp::run() {
|
---|
| 75 |
|
---|
| 76 | // Open Log File
|
---|
| 77 | // -------------
|
---|
[6339] | 78 | _logFile = new QFile(_logFileName);
|
---|
| 79 | if (_logFile->open(QIODevice::WriteOnly | QIODevice::Text)) {
|
---|
| 80 | _log = new QTextStream();
|
---|
| 81 | _log->setDevice(_logFile);
|
---|
| 82 | }
|
---|
[6340] | 83 | if (!_log) {
|
---|
| 84 | emit finished();
|
---|
[6339] | 85 | return;
|
---|
| 86 | }
|
---|
[6333] | 87 |
|
---|
[6339] | 88 | for (int ii = 0; ii < _sp3FileNames.size(); ii++) {
|
---|
[6342] | 89 | *_log << "SP3 File " << ii+1 << ": " << _sp3FileNames[ii] << endl;
|
---|
[6339] | 90 | }
|
---|
| 91 | if (_sp3FileNames.size() != 2) {
|
---|
| 92 | *_log << "ERROR: sp3Comp requires two input SP3 files" << endl;
|
---|
[6340] | 93 | emit finished();
|
---|
[6339] | 94 | return;
|
---|
| 95 | }
|
---|
[6335] | 96 |
|
---|
[6349] | 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 | }
|
---|
[6339] | 110 |
|
---|
[6333] | 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 |
|
---|
[6345] | 122 | // Satellite Index in clkSats set
|
---|
| 123 | ////////////////////////////////////////////////////////////////////////////////
|
---|
| 124 | int 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 | ////////////////////////////////////////////////////////////////////////////////
|
---|
| 137 | void 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 |
|
---|
[6348] | 192 | // Main Routine
|
---|
| 193 | ////////////////////////////////////////////////////////////////////////////////
|
---|
| 194 | void 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
|
---|
[6350] | 219 | epo->_dc[sat1->_prn] = sat1->_clk - sat2->_clk;
|
---|
[6348] | 220 | clkSats.insert(sat1->_prn);
|
---|
| 221 | }
|
---|
| 222 | }
|
---|
| 223 | }
|
---|
| 224 | }
|
---|
| 225 | if (epochOK) {
|
---|
| 226 | epochs.push_back(epo);
|
---|
[6349] | 227 | cout << "OK: " << string(epo->_tt) << endl;
|
---|
[6348] | 228 | }
|
---|
| 229 | else {
|
---|
| 230 | delete epo;
|
---|
| 231 | }
|
---|
[6349] | 232 | break;
|
---|
[6348] | 233 | }
|
---|
| 234 | }
|
---|
| 235 | }
|
---|
| 236 |
|
---|
[6349] | 237 | cout << "NUMEPO: " << epochs.size() << endl;
|
---|
| 238 |
|
---|
[6348] | 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 | }
|
---|