| 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 "sp3Comp.h"
|
|---|
| 43 | #include "bnccore.h"
|
|---|
| 44 | #include "bncsettings.h"
|
|---|
| 45 | #include "bncutils.h"
|
|---|
| 46 |
|
|---|
| 47 | using namespace std;
|
|---|
| 48 |
|
|---|
| 49 | // Constructor
|
|---|
| 50 | ////////////////////////////////////////////////////////////////////////////
|
|---|
| 51 | t_sp3Comp::t_sp3Comp(QObject* parent) : QThread(parent) {
|
|---|
| 52 |
|
|---|
| 53 | bncSettings settings;
|
|---|
| 54 | _sp3FileNames = settings.value("sp3CompFile").toString().split(',', QString::SkipEmptyParts);
|
|---|
| 55 | for (int ii = 0; ii < _sp3FileNames.size(); ii++) {
|
|---|
| 56 | expandEnvVar(_sp3FileNames[ii]);
|
|---|
| 57 | }
|
|---|
| 58 | _logFileName = settings.value("sp3CompOutLogFile").toString(); expandEnvVar(_logFileName);
|
|---|
| 59 | _logFile = 0;
|
|---|
| 60 | _log = 0;
|
|---|
| 61 | }
|
|---|
| 62 |
|
|---|
| 63 | // Destructor
|
|---|
| 64 | ////////////////////////////////////////////////////////////////////////////
|
|---|
| 65 | t_sp3Comp::~t_sp3Comp() {
|
|---|
| 66 | delete _log;
|
|---|
| 67 | delete _logFile;
|
|---|
| 68 | }
|
|---|
| 69 |
|
|---|
| 70 | //
|
|---|
| 71 | ////////////////////////////////////////////////////////////////////////////
|
|---|
| 72 | void t_sp3Comp::run() {
|
|---|
| 73 |
|
|---|
| 74 | // Open Log File
|
|---|
| 75 | // -------------
|
|---|
| 76 | _logFile = new QFile(_logFileName);
|
|---|
| 77 | if (_logFile->open(QIODevice::WriteOnly | QIODevice::Text)) {
|
|---|
| 78 | _log = new QTextStream();
|
|---|
| 79 | _log->setDevice(_logFile);
|
|---|
| 80 | }
|
|---|
| 81 | if (!_log) {
|
|---|
| 82 | emit finished();
|
|---|
| 83 | return;
|
|---|
| 84 | }
|
|---|
| 85 |
|
|---|
| 86 | for (int ii = 0; ii < _sp3FileNames.size(); ii++) {
|
|---|
| 87 | *_log << "SP3 File " << ii+1 << ": " << _sp3FileNames[ii] << endl;
|
|---|
| 88 | }
|
|---|
| 89 | if (_sp3FileNames.size() != 2) {
|
|---|
| 90 | *_log << "ERROR: sp3Comp requires two input SP3 files" << endl;
|
|---|
| 91 | emit finished();
|
|---|
| 92 | return;
|
|---|
| 93 | }
|
|---|
| 94 |
|
|---|
| 95 |
|
|---|
| 96 | // Exit (thread)
|
|---|
| 97 | // -------------
|
|---|
| 98 | if (BNC_CORE->mode() != t_bncCore::interactive) {
|
|---|
| 99 | qApp->exit(0);
|
|---|
| 100 | }
|
|---|
| 101 | else {
|
|---|
| 102 | emit finished();
|
|---|
| 103 | deleteLater();
|
|---|
| 104 | }
|
|---|
| 105 | }
|
|---|
| 106 |
|
|---|
| 107 | // Satellite Index in clkSats set
|
|---|
| 108 | ////////////////////////////////////////////////////////////////////////////////
|
|---|
| 109 | int t_sp3Comp::satIndex(const set<t_prn>& clkSats, const t_prn& prn) const {
|
|---|
| 110 | int ret = 0;
|
|---|
| 111 | for (set<t_prn>::const_iterator it = clkSats.begin(); it != clkSats.end(); it++) {
|
|---|
| 112 | if ( *it == prn) {
|
|---|
| 113 | return ret;
|
|---|
| 114 | }
|
|---|
| 115 | ++ret;
|
|---|
| 116 | }
|
|---|
| 117 | throw "satellite not found " + prn.toString();
|
|---|
| 118 | }
|
|---|
| 119 |
|
|---|
| 120 | // Estimate Clock Offsets
|
|---|
| 121 | ////////////////////////////////////////////////////////////////////////////////
|
|---|
| 122 | void t_sp3Comp::processClocks(const set<t_prn>& clkSats, vector<t_epoch*>& epochs,
|
|---|
| 123 | map<string, t_stat>& stat) const {
|
|---|
| 124 |
|
|---|
| 125 | if (clkSats.size() == 0) {
|
|---|
| 126 | return;
|
|---|
| 127 | }
|
|---|
| 128 |
|
|---|
| 129 | int nPar = epochs.size() + clkSats.size();
|
|---|
| 130 | SymmetricMatrix NN(nPar); NN = 0.0;
|
|---|
| 131 | ColumnVector bb(nPar); bb = 0.0;
|
|---|
| 132 |
|
|---|
| 133 | // Create Matrix A'A and vector b
|
|---|
| 134 | // ------------------------------
|
|---|
| 135 | for (unsigned ie = 0; ie < epochs.size(); ie++) {
|
|---|
| 136 | /// const bncTime& tt = epochs[ie]->_tt;
|
|---|
| 137 | const map<t_prn, double>& dc = epochs[ie]->_dc;
|
|---|
| 138 | Matrix AA(dc.size(), nPar); AA = 0.0;
|
|---|
| 139 | ColumnVector ll(dc.size()); ll = 0.0;
|
|---|
| 140 | map<t_prn, double>::const_iterator it; int ii;
|
|---|
| 141 | for (it = dc.begin(), ii = 0; it != dc.end(); it++, ii++) {
|
|---|
| 142 | const t_prn& prn = it->first;
|
|---|
| 143 | int index = epochs.size() + satIndex(clkSats, prn);
|
|---|
| 144 | AA[ii][ie] = 1.0; // epoch-specfic offset (common for all satellites)
|
|---|
| 145 | AA[ii][index] = 1.0; // satellite-specific offset (common for all epochs)
|
|---|
| 146 | ll[ii] = it->second;
|
|---|
| 147 | }
|
|---|
| 148 | SymmetricMatrix dN; dN << AA.t() * AA;
|
|---|
| 149 | NN += dN;
|
|---|
| 150 | bb += AA.t() * ll;
|
|---|
| 151 | }
|
|---|
| 152 |
|
|---|
| 153 | // Regularize NN
|
|---|
| 154 | // -------------
|
|---|
| 155 | RowVector HH(nPar);
|
|---|
| 156 | HH.columns(1, epochs.size()) = 0.0;
|
|---|
| 157 | HH.columns(epochs.size()+1, nPar) = 1.0;
|
|---|
| 158 | SymmetricMatrix dN; dN << HH.t() * HH;
|
|---|
| 159 | NN += dN;
|
|---|
| 160 |
|
|---|
| 161 | // Estimate Parameters
|
|---|
| 162 | // -------------------
|
|---|
| 163 | ColumnVector xx = NN.i() * bb;
|
|---|
| 164 |
|
|---|
| 165 | // Compute clock residuals
|
|---|
| 166 | // -----------------------
|
|---|
| 167 | for (unsigned ie = 0; ie < epochs.size(); ie++) {
|
|---|
| 168 | // const bncTime& tt = epochs[ie]->_tt;
|
|---|
| 169 | map<t_prn, double>& dc = epochs[ie]->_dc;
|
|---|
| 170 | for (map<t_prn, double>::iterator it = dc.begin(); it != dc.end(); it++) {
|
|---|
| 171 | const t_prn& prn = it->first;
|
|---|
| 172 | int index = epochs.size() + satIndex(clkSats, prn);
|
|---|
| 173 | dc[prn] = it->second - xx[ie] - xx[index];
|
|---|
| 174 | stat[prn.toString()]._offset = xx[index];
|
|---|
| 175 | }
|
|---|
| 176 | }
|
|---|
| 177 | }
|
|---|
| 178 |
|
|---|
| 179 | //// Program
|
|---|
| 180 | //////////////////////////////////////////////////////////////////////////////////
|
|---|
| 181 | //int main(int argc, char* argv[]) {
|
|---|
| 182 | //
|
|---|
| 183 | // t_sysout* sysout = new t_sysout("", 0, false);
|
|---|
| 184 | // rtnet_core_log = sysout->getSysoutCore();
|
|---|
| 185 | //
|
|---|
| 186 | // // Parse Input Options
|
|---|
| 187 | // // -------------------
|
|---|
| 188 | // map<string, string> OPT;
|
|---|
| 189 | // parseCmdLine(argc, argv, OPT);
|
|---|
| 190 | // if (OPT.find("sp3File1") == OPT.end() || OPT.find("sp3File2") == OPT.end()) {
|
|---|
| 191 | // cerr << "usage: sp3comp --sp3File1 <fileName> --sp3File2 <fileName> " << endl;
|
|---|
| 192 | // return 1;
|
|---|
| 193 | // }
|
|---|
| 194 | //
|
|---|
| 195 | // // Synchronize reading of two sp3 files
|
|---|
| 196 | // // ------------------------------------
|
|---|
| 197 | // vector<t_prn> prnList;
|
|---|
| 198 | // for (unsigned prn = 1; prn <= t_prn::MAXPRN; prn++) {
|
|---|
| 199 | // prnList.push_back(t_prn(prn));
|
|---|
| 200 | // }
|
|---|
| 201 | // t_sp3 in1(prnList); if (in1.assignFile(OPT["sp3File1"].c_str())) return 1;
|
|---|
| 202 | // t_sp3 in2(prnList); if (in2.assignFile(OPT["sp3File2"].c_str())) return 1;
|
|---|
| 203 | //
|
|---|
| 204 | // vector<t_epoch*> epochs;
|
|---|
| 205 | // set<t_prn> clkSats;
|
|---|
| 206 | // while (true) {
|
|---|
| 207 | // in1.readNextEpoch();
|
|---|
| 208 | // bncTime tt = in1.etime();
|
|---|
| 209 | // if (!tt.valid()) {
|
|---|
| 210 | // break;
|
|---|
| 211 | // }
|
|---|
| 212 | // if (in2.readEpoch(tt) == t_irc::success) {
|
|---|
| 213 | // t_epoch* epo = new t_epoch; epo->_tt = tt;
|
|---|
| 214 | // bool epochOK = false;
|
|---|
| 215 | // for (unsigned iPrn = 1; iPrn <= t_prn::MAXPRN; iPrn++) {
|
|---|
| 216 | // t_prn prn(iPrn);
|
|---|
| 217 | // ColumnVector xyz1(3), xyz2(3);
|
|---|
| 218 | // ColumnVector vel1(3), vel2(3);
|
|---|
| 219 | // double clk1, clk2;
|
|---|
| 220 | // bool posSet1, posSet2;
|
|---|
| 221 | // bool clkSet1, clkSet2;
|
|---|
| 222 | // bool velSet1, velSet2;
|
|---|
| 223 | // if (in1.position(prn, xyz1[0], xyz1[1], xyz1[2], posSet1, clk1, clkSet1,
|
|---|
| 224 | // vel1[0], vel1[1], vel1[2], velSet1) == 0 &&
|
|---|
| 225 | // in2.position(prn, xyz2[0], xyz2[1], xyz2[2], posSet2, clk2, clkSet2,
|
|---|
| 226 | // vel1[0], vel1[1], vel1[2], velSet1) == 0) {
|
|---|
| 227 | // if (posSet1 && posSet2) {
|
|---|
| 228 | // epochOK = true;
|
|---|
| 229 | // epo->_dr[prn] = xyz1 - xyz2;
|
|---|
| 230 | // epo->_xyz[prn] = xyz1;
|
|---|
| 231 | // if (clkSet1 && clkSet2) {
|
|---|
| 232 | // epo->_dc[prn] = (clk1 - clk2) * t_genConst::c;
|
|---|
| 233 | // clkSats.insert(prn);
|
|---|
| 234 | // }
|
|---|
| 235 | // }
|
|---|
| 236 | // }
|
|---|
| 237 | // }
|
|---|
| 238 | // if (epochOK) {
|
|---|
| 239 | // epochs.push_back(epo);
|
|---|
| 240 | // }
|
|---|
| 241 | // else {
|
|---|
| 242 | // delete epo;
|
|---|
| 243 | // }
|
|---|
| 244 | // }
|
|---|
| 245 | // if (in1.finished()) {
|
|---|
| 246 | // break;
|
|---|
| 247 | // }
|
|---|
| 248 | // }
|
|---|
| 249 | //
|
|---|
| 250 | // // Transform xyz into radial, along-track, and out-of-plane
|
|---|
| 251 | // // --------------------------------------------------------
|
|---|
| 252 | // for (unsigned ie = 0; ie < epochs.size(); ie++) {
|
|---|
| 253 | // t_epoch* epoch = epochs[ie];
|
|---|
| 254 | // t_epoch* epoch2 = 0;
|
|---|
| 255 | // if (ie == 0 && epochs.size() > 1) {
|
|---|
| 256 | // epoch2 = epochs[ie+1];
|
|---|
| 257 | // }
|
|---|
| 258 | // else {
|
|---|
| 259 | // epoch2 = epochs[ie-1];
|
|---|
| 260 | // }
|
|---|
| 261 | // double dt = epoch->_tt - epoch2->_tt;
|
|---|
| 262 | // map<t_prn, ColumnVector>& dr = epoch->_dr;
|
|---|
| 263 | // map<t_prn, ColumnVector>& xyz = epoch->_xyz;
|
|---|
| 264 | // map<t_prn, ColumnVector>& xyz2 = epoch2->_xyz;
|
|---|
| 265 | // for (map<t_prn, ColumnVector>::const_iterator it = dr.begin(); it != dr.end(); it++) {
|
|---|
| 266 | // const t_prn& prn = it->first;
|
|---|
| 267 | // if (xyz2.find(prn) != xyz2.end()) {
|
|---|
| 268 | // const ColumnVector dx = dr[prn];
|
|---|
| 269 | // const ColumnVector& x1 = xyz[prn];
|
|---|
| 270 | // const ColumnVector& x2 = xyz2[prn];
|
|---|
| 271 | // ColumnVector vel = (x1 - x2) / dt;
|
|---|
| 272 | // t_astro::XYZ_to_RSW(x1, vel, dx, dr[prn]);
|
|---|
| 273 | // }
|
|---|
| 274 | // else {
|
|---|
| 275 | // cerr << "not found: " << prn << endl;
|
|---|
| 276 | // }
|
|---|
| 277 | // }
|
|---|
| 278 | // }
|
|---|
| 279 | //
|
|---|
| 280 | // map<string, t_stat> stat;
|
|---|
| 281 | //
|
|---|
| 282 | // // Estimate Clock Offsets
|
|---|
| 283 | // // ----------------------
|
|---|
| 284 | // processClocks(clkSats, epochs, stat);
|
|---|
| 285 | //
|
|---|
| 286 | // // Print Residuals
|
|---|
| 287 | // // ---------------
|
|---|
| 288 | // const string all = "ZZZ";
|
|---|
| 289 | //
|
|---|
| 290 | // cout.setf(ios::fixed);
|
|---|
| 291 | // cout << "!\n! MJD PRN radial along out clk clkRed iPRN"
|
|---|
| 292 | // "\n! ----------------------------------------------------------------\n";
|
|---|
| 293 | // for (unsigned ii = 0; ii < epochs.size(); ii++) {
|
|---|
| 294 | // const t_epoch* epo = epochs[ii];
|
|---|
| 295 | // const map<t_prn, ColumnVector>& dr = epochs[ii]->_dr;
|
|---|
| 296 | // const map<t_prn, double>& dc = epochs[ii]->_dc;
|
|---|
| 297 | // for (map<t_prn, ColumnVector>::const_iterator it = dr.begin(); it != dr.end(); it++) {
|
|---|
| 298 | // const t_prn& prn = it->first;
|
|---|
| 299 | // const ColumnVector& rao = it->second;
|
|---|
| 300 | // cout << setprecision(6) << epo->_tt.mjddec() << ' ' << prn << ' '
|
|---|
| 301 | // << setw(7) << setprecision(4) << rao[0] << ' '
|
|---|
| 302 | // << setw(7) << setprecision(4) << rao[1] << ' '
|
|---|
| 303 | // << setw(7) << setprecision(4) << rao[2] << " ";
|
|---|
| 304 | // stat[prn]._rao += SP(rao, rao); // Schur product
|
|---|
| 305 | // stat[prn]._nr += 1;
|
|---|
| 306 | // stat[all]._rao += SP(rao, rao);
|
|---|
| 307 | // stat[all]._nr += 1;
|
|---|
| 308 | // if (dc.find(prn) != dc.end()) {
|
|---|
| 309 | // double clkRes = dc.find(prn)->second;
|
|---|
| 310 | // double clkResRed = clkRes - it->second[0]; // clock minus radial component
|
|---|
| 311 | // cout << setw(7) << setprecision(4) << clkRes << ' '
|
|---|
| 312 | // << setw(7) << setprecision(4) << clkResRed;
|
|---|
| 313 | // stat[prn]._dc += clkRes * clkRes;
|
|---|
| 314 | // stat[prn]._dcRed += clkResRed * clkResRed;
|
|---|
| 315 | // stat[prn]._nc += 1;
|
|---|
| 316 | // stat[all]._dc += clkRes * clkRes;
|
|---|
| 317 | // stat[all]._dcRed += clkResRed * clkResRed;
|
|---|
| 318 | // stat[all]._nc += 1;
|
|---|
| 319 | // }
|
|---|
| 320 | // else {
|
|---|
| 321 | // cout << " . . ";
|
|---|
| 322 | // }
|
|---|
| 323 | // cout << " " << setw(2) << int(prn) << endl;
|
|---|
| 324 | // }
|
|---|
| 325 | // delete epo;
|
|---|
| 326 | // }
|
|---|
| 327 | //
|
|---|
| 328 | // // Print Summary
|
|---|
| 329 | // // -------------
|
|---|
| 330 | // cout << "!\n! RMS:\n";
|
|---|
| 331 | // for (map<string, t_stat>::iterator it = stat.begin(); it != stat.end(); it++) {
|
|---|
| 332 | // const string& prn = it->first;
|
|---|
| 333 | // t_stat& stat = it->second;
|
|---|
| 334 | // if (stat._nr > 0) {
|
|---|
| 335 | // stat._rao[0] = sqrt(stat._rao[0] / stat._nr);
|
|---|
| 336 | // stat._rao[1] = sqrt(stat._rao[1] / stat._nr);
|
|---|
| 337 | // stat._rao[2] = sqrt(stat._rao[2] / stat._nr);
|
|---|
| 338 | // if (prn == all) {
|
|---|
| 339 | // cout << "!\n! Total ";
|
|---|
| 340 | // }
|
|---|
| 341 | // else {
|
|---|
| 342 | // cout << "! " << prn << ' ';
|
|---|
| 343 | // }
|
|---|
| 344 | // cout << setw(7) << setprecision(4) << stat._rao[0] << ' '
|
|---|
| 345 | // << setw(7) << setprecision(4) << stat._rao[1] << ' '
|
|---|
| 346 | // << setw(7) << setprecision(4) << stat._rao[2] << " ";
|
|---|
| 347 | // if (stat._nc > 0) {
|
|---|
| 348 | // stat._dc = sqrt(stat._dc / stat._nc);
|
|---|
| 349 | // stat._dcRed = sqrt(stat._dcRed / stat._nc);
|
|---|
| 350 | // cout << setw(7) << setprecision(4) << stat._dc << ' '
|
|---|
| 351 | // << setw(7) << setprecision(4) << stat._dcRed;
|
|---|
| 352 | // if (prn != all) {
|
|---|
| 353 | // cout << " offset " << setw(7) << setprecision(4) << stat._offset;
|
|---|
| 354 | // }
|
|---|
| 355 | // }
|
|---|
| 356 | // else {
|
|---|
| 357 | // cout << " . . ";
|
|---|
| 358 | // }
|
|---|
| 359 | // cout << endl;
|
|---|
| 360 | // }
|
|---|
| 361 | // }
|
|---|
| 362 | //
|
|---|
| 363 | // return 0;
|
|---|
| 364 | //}
|
|---|
