source: ntrip/trunk/BNC/src/bncmodel.cpp@ 5808

Last change on this file since 5808 was 5808, checked in by mervart, 8 years ago
File size: 37.3 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: bncParam, bncModel
30 *
31 * Purpose: Model for PPP
32 *
33 * Author: L. Mervart
34 *
35 * Created: 01-Dec-2009
36 *
37 * Changes:
38 *
39 * -----------------------------------------------------------------------*/
40
41#include <iomanip>
42#include <cmath>
43#include <newmatio.h>
44#include <sstream>
45
46#include "bncmodel.h"
47#include "bncpppclient.h"
48#include "bnccore.h"
49#include "bncpppclient.h"
50#include "bancroft.h"
51#include "bncutils.h"
52#include "PPP/pppModel.h"
53#include "bncantex.h"
54#include "pppopt.h"
55
56using namespace BNC;
57using namespace std;
58
59const unsigned MINOBS = 5;
60const double MINELE = 10.0 * M_PI / 180.0;
61const double MAXRES_CODE = 15.0;
62const double MAXRES_PHASE_GPS = 0.04;
63const double MAXRES_PHASE_GLONASS = 0.08;
64const double GLONASS_WEIGHT_FACTOR = 5.0;
65
66// Constructor
67////////////////////////////////////////////////////////////////////////////
68bncParam::bncParam(bncParam::parType typeIn, int indexIn,
69 const QString& prnIn) {
70 type = typeIn;
71 index = indexIn;
72 prn = prnIn;
73 index_old = 0;
74 xx = 0.0;
75 numEpo = 0;
76}
77
78// Destructor
79////////////////////////////////////////////////////////////////////////////
80bncParam::~bncParam() {
81}
82
83// Partial
84////////////////////////////////////////////////////////////////////////////
85double bncParam::partial(t_satData* satData, bool phase) {
86
87 Tracer tracer("bncParam::partial");
88
89 // Coordinates
90 // -----------
91 if (type == CRD_X) {
92 return (xx - satData->xx(1)) / satData->rho;
93 }
94 else if (type == CRD_Y) {
95 return (xx - satData->xx(2)) / satData->rho;
96 }
97 else if (type == CRD_Z) {
98 return (xx - satData->xx(3)) / satData->rho;
99 }
100
101 // Receiver Clocks
102 // ---------------
103 else if (type == RECCLK) {
104 return 1.0;
105 }
106
107 // Troposphere
108 // -----------
109 else if (type == TROPO) {
110 return 1.0 / sin(satData->eleSat);
111 }
112
113 // Glonass Offset
114 // --------------
115 else if (type == GLONASS_OFFSET) {
116 if (satData->prn[0] == 'R') {
117 return 1.0;
118 }
119 else {
120 return 0.0;
121 }
122 }
123
124 // Galileo Offset
125 // --------------
126 else if (type == GALILEO_OFFSET) {
127 if (satData->prn[0] == 'E') {
128 return 1.0;
129 }
130 else {
131 return 0.0;
132 }
133 }
134
135 // Ambiguities
136 // -----------
137 else if (type == AMB_L3) {
138 if (phase && satData->prn == prn) {
139 return 1.0;
140 }
141 else {
142 return 0.0;
143 }
144 }
145
146 // Default return
147 // --------------
148 return 0.0;
149}
150
151// Constructor
152////////////////////////////////////////////////////////////////////////////
153bncModel::bncModel(bncPPPclient* pppClient) {
154
155 _pppClient = pppClient;
156 _staID = pppClient->staID();
157 _opt = pppClient->opt();
158
159 // NMEA Output
160 // -----------
161 if (_opt->nmeaFile.isEmpty()) {
162 _nmeaFile = 0;
163 _nmeaStream = 0;
164 }
165 else {
166 QString hlpName = _opt->nmeaFile; expandEnvVar(hlpName);
167 _nmeaFile = new QFile(hlpName);
168 if (_opt->rnxAppend) {
169 _nmeaFile->open(QIODevice::WriteOnly | QIODevice::Append);
170 }
171 else {
172 _nmeaFile->open(QIODevice::WriteOnly);
173 }
174 _nmeaStream = new QTextStream();
175 _nmeaStream->setDevice(_nmeaFile);
176 }
177
178 // Antenna Name, ANTEX File
179 // ------------------------
180 _antex = 0;
181 if (!_opt->antexFile.isEmpty()) {
182 _antex = new bncAntex();
183 if (_antex->readFile(_opt->antexFile) != success) {
184 _pppClient->emitNewMessage("wrong ANTEX file", true);
185 delete _antex;
186 _antex = 0;
187 }
188 }
189
190 _tides = new t_tides;
191
192 // Bancroft Coordinates
193 // --------------------
194 _xcBanc.ReSize(4); _xcBanc = 0.0;
195 _ellBanc.ReSize(3); _ellBanc = 0.0;
196
197 // Save copy of data (used in outlier detection)
198 // ---------------------------------------------
199 _epoData_sav = new t_epoData();
200}
201
202// Destructor
203////////////////////////////////////////////////////////////////////////////
204bncModel::~bncModel() {
205 delete _nmeaStream;
206 delete _nmeaFile;
207 for (int ii = 0; ii < _posAverage.size(); ++ii) {
208 delete _posAverage[ii];
209 }
210 delete _antex;
211 for (int iPar = 1; iPar <= _params.size(); iPar++) {
212 delete _params[iPar-1];
213 }
214 for (int iPar = 1; iPar <= _params_sav.size(); iPar++) {
215 delete _params_sav[iPar-1];
216 }
217 delete _epoData_sav;
218 delete _tides;
219}
220
221// Reset Parameters and Variance-Covariance Matrix
222////////////////////////////////////////////////////////////////////////////
223void bncModel::reset() {
224
225 Tracer tracer("bncModel::reset");
226
227 double lastTrp = 0.0;
228 for (int ii = 0; ii < _params.size(); ii++) {
229 bncParam* pp = _params[ii];
230 if (pp->type == bncParam::TROPO) {
231 lastTrp = pp->xx;
232 }
233 delete pp;
234 }
235 _params.clear();
236
237 int nextPar = 0;
238 _params.push_back(new bncParam(bncParam::CRD_X, ++nextPar, ""));
239 _params.push_back(new bncParam(bncParam::CRD_Y, ++nextPar, ""));
240 _params.push_back(new bncParam(bncParam::CRD_Z, ++nextPar, ""));
241 _params.push_back(new bncParam(bncParam::RECCLK, ++nextPar, ""));
242 if (_opt->estTropo) {
243 _params.push_back(new bncParam(bncParam::TROPO, ++nextPar, ""));
244 }
245 if (_opt->useGlonass) {
246 _params.push_back(new bncParam(bncParam::GLONASS_OFFSET, ++nextPar, ""));
247 }
248 if (_opt->useGalileo) {
249 _params.push_back(new bncParam(bncParam::GALILEO_OFFSET, ++nextPar, ""));
250 }
251
252 _QQ.ReSize(_params.size());
253 _QQ = 0.0;
254 for (int iPar = 1; iPar <= _params.size(); iPar++) {
255 bncParam* pp = _params[iPar-1];
256 pp->xx = 0.0;
257 if (pp->isCrd()) {
258 _QQ(iPar,iPar) = _opt->sigCrd0 * _opt->sigCrd0;
259 }
260 else if (pp->type == bncParam::RECCLK) {
261 _QQ(iPar,iPar) = _opt->sigClk0 * _opt->sigClk0;
262 }
263 else if (pp->type == bncParam::TROPO) {
264 _QQ(iPar,iPar) = _opt->sigTrp0 * _opt->sigTrp0;
265 pp->xx = lastTrp;
266 }
267 else if (pp->type == bncParam::GLONASS_OFFSET) {
268 _QQ(iPar,iPar) = _opt->sigGlonassOffset0 * _opt->sigGlonassOffset0;
269 }
270 else if (pp->type == bncParam::GALILEO_OFFSET) {
271 _QQ(iPar,iPar) = _opt->sigGalileoOffset0 * _opt->sigGalileoOffset0;
272 }
273 }
274}
275
276// Bancroft Solution
277////////////////////////////////////////////////////////////////////////////
278t_irc bncModel::cmpBancroft(t_epoData* epoData) {
279
280 Tracer tracer("bncModel::cmpBancroft");
281
282 if (epoData->sizeSys('G') < MINOBS) {
283 _log += "bncModel::cmpBancroft: not enough data\n";
284 return failure;
285 }
286
287 Matrix BB(epoData->sizeSys('G'), 4);
288
289 QMapIterator<QString, t_satData*> it(epoData->satData);
290 int iObsBanc = 0;
291 while (it.hasNext()) {
292 it.next();
293 t_satData* satData = it.value();
294 if (satData->system() == 'G') {
295 ++iObsBanc;
296 QString prn = it.key();
297 BB(iObsBanc, 1) = satData->xx(1);
298 BB(iObsBanc, 2) = satData->xx(2);
299 BB(iObsBanc, 3) = satData->xx(3);
300 BB(iObsBanc, 4) = satData->P3 + satData->clk;
301 }
302 }
303
304 bancroft(BB, _xcBanc);
305
306 // Ellipsoidal Coordinates
307 // ------------------------
308 xyz2ell(_xcBanc.data(), _ellBanc.data());
309
310 // Compute Satellite Elevations
311 // ----------------------------
312 QMutableMapIterator<QString, t_satData*> im(epoData->satData);
313 while (im.hasNext()) {
314 im.next();
315 t_satData* satData = im.value();
316 cmpEle(satData);
317 if (satData->eleSat < MINELE) {
318 delete satData;
319 im.remove();
320 }
321 }
322
323 return success;
324}
325
326// Computed Value
327////////////////////////////////////////////////////////////////////////////
328double bncModel::cmpValue(t_satData* satData, bool phase) {
329
330 Tracer tracer("bncModel::cmpValue");
331
332 ColumnVector xRec(3);
333 xRec(1) = x();
334 xRec(2) = y();
335 xRec(3) = z();
336
337 double rho0 = (satData->xx - xRec).norm_Frobenius();
338 double dPhi = t_CST::omega * rho0 / t_CST::c;
339
340 xRec(1) = x() * cos(dPhi) - y() * sin(dPhi);
341 xRec(2) = y() * cos(dPhi) + x() * sin(dPhi);
342 xRec(3) = z();
343
344 xRec += _tides->displacement(_time, xRec);
345
346 satData->rho = (satData->xx - xRec).norm_Frobenius();
347
348 double tropDelay = t_tropo::delay_saast(xRec, satData->eleSat) +
349 trp() / sin(satData->eleSat);
350
351 double wind = 0.0;
352 if (phase) {
353 wind = windUp(satData->prn, satData->xx, xRec) * satData->lambda3;
354 }
355
356 double offset = 0.0;
357 if (satData->prn[0] == 'R') {
358 offset = Glonass_offset();
359 }
360 else if (satData->prn[0] == 'E') {
361 offset = Galileo_offset();
362 }
363
364 double phaseCenter = 0.0;
365 if (_antex) {
366 bool found;
367 phaseCenter = _antex->pco(_opt->antennaName, satData->eleSat, found);
368 if (!found) {
369 _pppClient->emitNewMessage("ANTEX: antenna >"
370 + _opt->antennaName.toAscii() + "< not found", true);
371 }
372 }
373
374 double antennaOffset = 0.0;
375 if (_opt->antEccSet()) {
376 double cosa = cos(satData->azSat);
377 double sina = sin(satData->azSat);
378 double cose = cos(satData->eleSat);
379 double sine = sin(satData->eleSat);
380 antennaOffset = -_opt->antEccNEU[0] * cosa*cose
381 -_opt->antEccNEU[1] * sina*cose
382 -_opt->antEccNEU[2] * sine;
383 }
384
385 return satData->rho + phaseCenter + antennaOffset + clk()
386 + offset - satData->clk + tropDelay + wind;
387}
388
389// Prediction Step of the Filter
390////////////////////////////////////////////////////////////////////////////
391void bncModel::predict(int iPhase, t_epoData* epoData) {
392
393 Tracer tracer("bncModel::predict");
394
395 if (iPhase == 0) {
396
397 bool firstCrd = false;
398 if (!_lastTimeOK.valid() || (_opt->maxSolGap > 0 && _time - _lastTimeOK > _opt->maxSolGap)) {
399 firstCrd = true;
400 _startTime = epoData->tt;
401 reset();
402 }
403
404 // Use different white noise for Quick-Start mode
405 // ----------------------------------------------
406 double sigCrdP_used = _opt->sigCrdP;
407 if ( _opt->quickStart > 0.0 && _opt->quickStart > (epoData->tt - _startTime) ) {
408 sigCrdP_used = 0.0;
409 }
410
411 // Predict Parameter values, add white noise
412 // -----------------------------------------
413 for (int iPar = 1; iPar <= _params.size(); iPar++) {
414 bncParam* pp = _params[iPar-1];
415
416 // Coordinates
417 // -----------
418 if (pp->type == bncParam::CRD_X) {
419 if (firstCrd) {
420 if (_opt->refCrdSet()) {
421 pp->xx = _opt->refCrd[0];
422 }
423 else {
424 pp->xx = _xcBanc(1);
425 }
426 }
427 _QQ(iPar,iPar) += sigCrdP_used * sigCrdP_used;
428 }
429 else if (pp->type == bncParam::CRD_Y) {
430 if (firstCrd) {
431 if (_opt->refCrdSet()) {
432 pp->xx = _opt->refCrd[1];
433 }
434 else {
435 pp->xx = _xcBanc(2);
436 }
437 }
438 _QQ(iPar,iPar) += sigCrdP_used * sigCrdP_used;
439 }
440 else if (pp->type == bncParam::CRD_Z) {
441 if (firstCrd) {
442 if (_opt->refCrdSet()) {
443 pp->xx = _opt->refCrd[2];
444 }
445 else {
446 pp->xx = _xcBanc(3);
447 }
448 }
449 _QQ(iPar,iPar) += sigCrdP_used * sigCrdP_used;
450 }
451
452 // Receiver Clocks
453 // ---------------
454 else if (pp->type == bncParam::RECCLK) {
455 pp->xx = _xcBanc(4);
456 for (int jj = 1; jj <= _params.size(); jj++) {
457 _QQ(iPar, jj) = 0.0;
458 }
459 _QQ(iPar,iPar) = _opt->sigClk0 * _opt->sigClk0;
460 }
461
462 // Tropospheric Delay
463 // ------------------
464 else if (pp->type == bncParam::TROPO) {
465 _QQ(iPar,iPar) += _opt->sigTrpP * _opt->sigTrpP;
466 }
467
468 // Glonass Offset
469 // --------------
470 else if (pp->type == bncParam::GLONASS_OFFSET) {
471 bool epoSpec = true;
472 if (epoSpec) {
473 pp->xx = 0.0;
474 for (int jj = 1; jj <= _params.size(); jj++) {
475 _QQ(iPar, jj) = 0.0;
476 }
477 _QQ(iPar,iPar) = _opt->sigGlonassOffset0 * _opt->sigGlonassOffset0;
478 }
479 else {
480 _QQ(iPar,iPar) += _opt->sigGlonassOffsetP * _opt->sigGlonassOffsetP;
481 }
482 }
483
484 // Galileo Offset
485 // --------------
486 else if (pp->type == bncParam::GALILEO_OFFSET) {
487 _QQ(iPar,iPar) += _opt->sigGalileoOffsetP * _opt->sigGalileoOffsetP;
488 }
489 }
490 }
491
492 // Add New Ambiguities if necessary
493 // --------------------------------
494 if (_opt->usePhase) {
495
496 // Make a copy of QQ and xx, set parameter indices
497 // -----------------------------------------------
498 SymmetricMatrix QQ_old = _QQ;
499
500 for (int iPar = 1; iPar <= _params.size(); iPar++) {
501 _params[iPar-1]->index_old = _params[iPar-1]->index;
502 _params[iPar-1]->index = 0;
503 }
504
505 // Remove Ambiguity Parameters without observations
506 // ------------------------------------------------
507 int iPar = 0;
508 QMutableVectorIterator<bncParam*> im(_params);
509 while (im.hasNext()) {
510 bncParam* par = im.next();
511 bool removed = false;
512 if (par->type == bncParam::AMB_L3) {
513 if (epoData->satData.find(par->prn) == epoData->satData.end()) {
514 removed = true;
515 delete par;
516 im.remove();
517 }
518 }
519 if (! removed) {
520 ++iPar;
521 par->index = iPar;
522 }
523 }
524
525 // Add new ambiguity parameters
526 // ----------------------------
527 QMapIterator<QString, t_satData*> it(epoData->satData);
528 while (it.hasNext()) {
529 it.next();
530 t_satData* satData = it.value();
531 addAmb(satData);
532 }
533
534 int nPar = _params.size();
535 _QQ.ReSize(nPar); _QQ = 0.0;
536 for (int i1 = 1; i1 <= nPar; i1++) {
537 bncParam* p1 = _params[i1-1];
538 if (p1->index_old != 0) {
539 _QQ(p1->index, p1->index) = QQ_old(p1->index_old, p1->index_old);
540 for (int i2 = 1; i2 <= nPar; i2++) {
541 bncParam* p2 = _params[i2-1];
542 if (p2->index_old != 0) {
543 _QQ(p1->index, p2->index) = QQ_old(p1->index_old, p2->index_old);
544 }
545 }
546 }
547 }
548
549 for (int ii = 1; ii <= nPar; ii++) {
550 bncParam* par = _params[ii-1];
551 if (par->index_old == 0) {
552 _QQ(par->index, par->index) = _opt->sigAmb0 * _opt->sigAmb0;
553 }
554 par->index_old = par->index;
555 }
556 }
557}
558
559// Update Step of the Filter (currently just a single-epoch solution)
560////////////////////////////////////////////////////////////////////////////
561t_irc bncModel::update(t_epoData* epoData) {
562
563 Tracer tracer("bncModel::update");
564
565 _log.clear();
566
567 _time = epoData->tt; // current epoch time
568
569 if (_opt->pppMode) {
570 _log += "Precise Point Positioning of Epoch "
571 + QByteArray(_time.timestr(1).c_str()) +
572 "\n---------------------------------------------------------------\n";
573 }
574 else {
575 _log += "Single Point Positioning of Epoch "
576 + QByteArray(_time.timestr(1).c_str()) +
577 "\n--------------------------------------------------------------\n";
578 }
579
580 // Outlier Detection Loop
581 // ----------------------
582 if (update_p(epoData) != success) {
583 _pppClient->emitNewMessage(_log, false);
584 return failure;
585 }
586
587 // Remember the Epoch-specific Results for the computation of means
588 // ----------------------------------------------------------------
589 pppPos* newPos = new pppPos;
590 newPos->time = epoData->tt;
591
592 // Set Solution Vector
593 // -------------------
594 ostringstream strB;
595 strB.setf(ios::fixed);
596 QVectorIterator<bncParam*> itPar(_params);
597 while (itPar.hasNext()) {
598 bncParam* par = itPar.next();
599
600 if (par->type == bncParam::RECCLK) {
601 strB << "\n clk = " << setw(10) << setprecision(3) << par->xx
602 << " +- " << setw(6) << setprecision(3)
603 << sqrt(_QQ(par->index,par->index));
604 }
605 else if (par->type == bncParam::AMB_L3) {
606 ++par->numEpo;
607 strB << "\n amb " << par->prn.toAscii().data() << " = "
608 << setw(10) << setprecision(3) << par->xx
609 << " +- " << setw(6) << setprecision(3)
610 << sqrt(_QQ(par->index,par->index))
611 << " nEpo = " << par->numEpo;
612 }
613 else if (par->type == bncParam::TROPO) {
614 ColumnVector xyz(3); xyz(1) = x(); xyz(2) = y(); xyz(3) = z();
615 double aprTrp = t_tropo::delay_saast(xyz, M_PI/2.0);
616 strB << "\n trp = " << par->prn.toAscii().data()
617 << setw(7) << setprecision(3) << aprTrp << " "
618 << setw(6) << setprecision(3) << showpos << par->xx << noshowpos
619 << " +- " << setw(6) << setprecision(3)
620 << sqrt(_QQ(par->index,par->index));
621 newPos->xnt[6] = aprTrp + par->xx;
622 }
623 else if (par->type == bncParam::GLONASS_OFFSET) {
624 strB << "\n offGlo = " << setw(10) << setprecision(3) << par->xx
625 << " +- " << setw(6) << setprecision(3)
626 << sqrt(_QQ(par->index,par->index));
627 }
628 else if (par->type == bncParam::GALILEO_OFFSET) {
629 strB << "\n offGal = " << setw(10) << setprecision(3) << par->xx
630 << " +- " << setw(6) << setprecision(3)
631 << sqrt(_QQ(par->index,par->index));
632 }
633 }
634 strB << '\n';
635 _log += strB.str().c_str();
636 _pppClient->emitNewMessage(_log, false);
637
638 // Final Message (both log file and screen)
639 // ----------------------------------------
640 ostringstream strC;
641 strC.setf(ios::fixed);
642 strC << _staID.data() << " PPP "
643 << epoData->tt.timestr(1) << " " << epoData->sizeAll() << " "
644 << setw(14) << setprecision(3) << x() << " +- "
645 << setw(6) << setprecision(3) << sqrt(_QQ(1,1)) << " "
646 << setw(14) << setprecision(3) << y() << " +- "
647 << setw(6) << setprecision(3) << sqrt(_QQ(2,2)) << " "
648 << setw(14) << setprecision(3) << z() << " +- "
649 << setw(6) << setprecision(3) << sqrt(_QQ(3,3));
650
651 // NEU Output
652 // ----------
653 if (_opt->refCrdSet()) {
654 newPos->xnt[0] = x() - _opt->refCrd[0];
655 newPos->xnt[1] = y() - _opt->refCrd[1];
656 newPos->xnt[2] = z() - _opt->refCrd[2];
657
658 double ellRef[3];
659 xyz2ell(_opt->refCrd, ellRef);
660 xyz2neu(ellRef, newPos->xnt, &newPos->xnt[3]);
661
662 strC << " NEU "
663 << setw(8) << setprecision(3) << newPos->xnt[3] << " "
664 << setw(8) << setprecision(3) << newPos->xnt[4] << " "
665 << setw(8) << setprecision(3) << newPos->xnt[5] << endl;
666
667 }
668
669 _pppClient->emitNewMessage(QByteArray(strC.str().c_str()), true);
670
671 if (_opt->pppAverage == 0.0) {
672 delete newPos;
673 }
674 else {
675
676 _posAverage.push_back(newPos);
677
678 // Compute the Mean
679 // ----------------
680 ColumnVector mean(7); mean = 0.0;
681
682 QMutableVectorIterator<pppPos*> it(_posAverage);
683 while (it.hasNext()) {
684 pppPos* pp = it.next();
685 if ( (epoData->tt - pp->time) >= _opt->pppAverage ) {
686 delete pp;
687 it.remove();
688 }
689 else {
690 for (int ii = 0; ii < 7; ++ii) {
691 mean[ii] += pp->xnt[ii];
692 }
693 }
694 }
695
696 int nn = _posAverage.size();
697
698 if (nn > 0) {
699
700 mean /= nn;
701
702 // Compute the Deviation
703 // ---------------------
704 ColumnVector std(7); std = 0.0;
705 QVectorIterator<pppPos*> it2(_posAverage);
706 while (it2.hasNext()) {
707 pppPos* pp = it2.next();
708 for (int ii = 0; ii < 7; ++ii) {
709 std[ii] += (pp->xnt[ii] - mean[ii]) * (pp->xnt[ii] - mean[ii]);
710 }
711 }
712 for (int ii = 0; ii < 7; ++ii) {
713 std[ii] = sqrt(std[ii] / nn);
714 }
715
716 if (_opt->refCrdSet()) {
717 ostringstream strD; strD.setf(ios::fixed);
718 strD << _staID.data() << " AVE-XYZ "
719 << epoData->tt.timestr(1) << " "
720 << setw(13) << setprecision(3) << mean[0] + _opt->refCrd[0] << " +- "
721 << setw(6) << setprecision(3) << std[0] << " "
722 << setw(14) << setprecision(3) << mean[1] + _opt->refCrd[1] << " +- "
723 << setw(6) << setprecision(3) << std[1] << " "
724 << setw(14) << setprecision(3) << mean[2] + _opt->refCrd[2] << " +- "
725 << setw(6) << setprecision(3) << std[2];
726 _pppClient->emitNewMessage(QByteArray(strD.str().c_str()), true);
727
728 ostringstream strE; strE.setf(ios::fixed);
729 strE << _staID.data() << " AVE-NEU "
730 << epoData->tt.timestr(1) << " "
731 << setw(13) << setprecision(3) << mean[3] << " +- "
732 << setw(6) << setprecision(3) << std[3] << " "
733 << setw(14) << setprecision(3) << mean[4] << " +- "
734 << setw(6) << setprecision(3) << std[4] << " "
735 << setw(14) << setprecision(3) << mean[5] << " +- "
736 << setw(6) << setprecision(3) << std[5];
737 _pppClient->emitNewMessage(QByteArray(strE.str().c_str()), true);
738
739 if (_opt->estTropo) {
740 ostringstream strF; strF.setf(ios::fixed);
741 strF << _staID.data() << " AVE-TRP "
742 << epoData->tt.timestr(1) << " "
743 << setw(13) << setprecision(3) << mean[6] << " +- "
744 << setw(6) << setprecision(3) << std[6] << endl;
745 _pppClient->emitNewMessage(QByteArray(strF.str().c_str()), true);
746 }
747 }
748 }
749 }
750
751 // NMEA Output
752 // -----------
753 double xyz[3];
754 xyz[0] = x();
755 xyz[1] = y();
756 xyz[2] = z();
757 double ell[3];
758 xyz2ell(xyz, ell);
759 double phiDeg = ell[0] * 180 / M_PI;
760 double lamDeg = ell[1] * 180 / M_PI;
761
762 char phiCh = 'N';
763 if (phiDeg < 0) {
764 phiDeg = -phiDeg;
765 phiCh = 'S';
766 }
767 char lamCh = 'E';
768 if (lamDeg < 0) {
769 lamDeg = -lamDeg;
770 lamCh = 'W';
771 }
772
773 string datestr = epoData->tt.datestr(0); // yyyymmdd
774 ostringstream strRMC;
775 strRMC.setf(ios::fixed);
776 strRMC << "GPRMC,"
777 << epoData->tt.timestr(0,0) << ",A,"
778 << setw(2) << setfill('0') << int(phiDeg)
779 << setw(6) << setprecision(3) << setfill('0')
780 << fmod(60*phiDeg,60) << ',' << phiCh << ','
781 << setw(3) << setfill('0') << int(lamDeg)
782 << setw(6) << setprecision(3) << setfill('0')
783 << fmod(60*lamDeg,60) << ',' << lamCh << ",,,"
784 << datestr[6] << datestr[7] << datestr[4] << datestr[5]
785 << datestr[2] << datestr[3] << ",,";
786
787 writeNMEAstr(QString(strRMC.str().c_str()));
788
789 double dop = 2.0; // TODO
790
791 ostringstream strGGA;
792 strGGA.setf(ios::fixed);
793 strGGA << "GPGGA,"
794 << epoData->tt.timestr(0,0) << ','
795 << setw(2) << setfill('0') << int(phiDeg)
796 << setw(10) << setprecision(7) << setfill('0')
797 << fmod(60*phiDeg,60) << ',' << phiCh << ','
798 << setw(3) << setfill('0') << int(lamDeg)
799 << setw(10) << setprecision(7) << setfill('0')
800 << fmod(60*lamDeg,60) << ',' << lamCh
801 << ",1," << setw(2) << setfill('0') << epoData->sizeAll() << ','
802 << setw(3) << setprecision(1) << dop << ','
803 << setprecision(3) << ell[2] << ",M,0.0,M,,";
804
805 writeNMEAstr(QString(strGGA.str().c_str()));
806
807 _lastTimeOK = _time; // remember time of last successful update
808 return success;
809}
810
811// Outlier Detection
812////////////////////////////////////////////////////////////////////////////
813QString bncModel::outlierDetection(int iPhase, const ColumnVector& vv,
814 QMap<QString, t_satData*>& satData) {
815
816 Tracer tracer("bncModel::outlierDetection");
817
818 QString prnGPS;
819 QString prnGlo;
820 double maxResGPS = 0.0;
821 double maxResGlo = 0.0;
822 findMaxRes(vv, satData, prnGPS, prnGlo, maxResGPS, maxResGlo);
823
824 if (iPhase == 1) {
825 if (maxResGlo > MAXRES_PHASE_GLONASS) {
826 _log += "Outlier Phase " + prnGlo + " "
827 + QByteArray::number(maxResGlo, 'f', 3) + "\n";
828 return prnGlo;
829 }
830 else if (maxResGPS > MAXRES_PHASE_GPS) {
831 _log += "Outlier Phase " + prnGPS + " "
832 + QByteArray::number(maxResGPS, 'f', 3) + "\n";
833 return prnGPS;
834 }
835 }
836 else if (iPhase == 0 && maxResGPS > MAXRES_CODE) {
837 _log += "Outlier Code " + prnGPS + " "
838 + QByteArray::number(maxResGPS, 'f', 3) + "\n";
839 return prnGPS;
840 }
841
842 return QString();
843}
844
845//
846////////////////////////////////////////////////////////////////////////////
847void bncModel::writeNMEAstr(const QString& nmStr) {
848
849 Tracer tracer("bncModel::writeNMEAstr");
850
851 unsigned char XOR = 0;
852 for (int ii = 0; ii < nmStr.length(); ii++) {
853 XOR ^= (unsigned char) nmStr[ii].toAscii();
854 }
855
856 QString outStr = '$' + nmStr
857 + QString("*%1\n").arg(int(XOR), 0, 16).toUpper();
858
859 if (_nmeaStream) {
860 *_nmeaStream << outStr;
861 _nmeaStream->flush();
862 }
863
864 _pppClient->emitNewNMEAstr(outStr.toAscii());
865}
866
867// Phase Wind-Up Correction
868///////////////////////////////////////////////////////////////////////////
869double bncModel::windUp(const QString& prn, const ColumnVector& rSat,
870 const ColumnVector& rRec) {
871
872 Tracer tracer("bncModel::windUp");
873
874 double Mjd = _time.mjd() + _time.daysec() / 86400.0;
875
876 // First time - initialize to zero
877 // -------------------------------
878 if (!_windUpTime.contains(prn)) {
879 _windUpSum[prn] = 0.0;
880 }
881
882 // Compute the correction for new time
883 // -----------------------------------
884 if (!_windUpTime.contains(prn) || _windUpTime[prn] != Mjd) {
885 _windUpTime[prn] = Mjd;
886
887 // Unit Vector GPS Satellite --> Receiver
888 // --------------------------------------
889 ColumnVector rho = rRec - rSat;
890 rho /= rho.norm_Frobenius();
891
892 // GPS Satellite unit Vectors sz, sy, sx
893 // -------------------------------------
894 ColumnVector sz = -rSat / rSat.norm_Frobenius();
895
896 ColumnVector xSun = t_astro::Sun(Mjd);
897 xSun /= xSun.norm_Frobenius();
898
899 ColumnVector sy = crossproduct(sz, xSun);
900 ColumnVector sx = crossproduct(sy, sz);
901
902 // Effective Dipole of the GPS Satellite Antenna
903 // ---------------------------------------------
904 ColumnVector dipSat = sx - rho * DotProduct(rho,sx)
905 - crossproduct(rho, sy);
906
907 // Receiver unit Vectors rx, ry
908 // ----------------------------
909 ColumnVector rx(3);
910 ColumnVector ry(3);
911
912 double recEll[3]; xyz2ell(rRec.data(), recEll) ;
913 double neu[3];
914
915 neu[0] = 1.0;
916 neu[1] = 0.0;
917 neu[2] = 0.0;
918 neu2xyz(recEll, neu, rx.data());
919
920 neu[0] = 0.0;
921 neu[1] = -1.0;
922 neu[2] = 0.0;
923 neu2xyz(recEll, neu, ry.data());
924
925 // Effective Dipole of the Receiver Antenna
926 // ----------------------------------------
927 ColumnVector dipRec = rx - rho * DotProduct(rho,rx)
928 + crossproduct(rho, ry);
929
930 // Resulting Effect
931 // ----------------
932 double alpha = DotProduct(dipSat,dipRec) /
933 (dipSat.norm_Frobenius() * dipRec.norm_Frobenius());
934
935 if (alpha > 1.0) alpha = 1.0;
936 if (alpha < -1.0) alpha = -1.0;
937
938 double dphi = acos(alpha) / 2.0 / M_PI; // in cycles
939
940 if ( DotProduct(rho, crossproduct(dipSat, dipRec)) < 0.0 ) {
941 dphi = -dphi;
942 }
943
944 _windUpSum[prn] = floor(_windUpSum[prn] - dphi + 0.5) + dphi;
945 }
946
947 return _windUpSum[prn];
948}
949
950//
951///////////////////////////////////////////////////////////////////////////
952void bncModel::cmpEle(t_satData* satData) {
953 Tracer tracer("bncModel::cmpEle");
954 ColumnVector rr = satData->xx - _xcBanc.Rows(1,3);
955 double rho = rr.norm_Frobenius();
956
957 double neu[3];
958 xyz2neu(_ellBanc.data(), rr.data(), neu);
959
960 satData->eleSat = acos( sqrt(neu[0]*neu[0] + neu[1]*neu[1]) / rho );
961 if (neu[2] < 0) {
962 satData->eleSat *= -1.0;
963 }
964 satData->azSat = atan2(neu[1], neu[0]);
965}
966
967//
968///////////////////////////////////////////////////////////////////////////
969void bncModel::addAmb(t_satData* satData) {
970 Tracer tracer("bncModel::addAmb");
971 bool found = false;
972 for (int iPar = 1; iPar <= _params.size(); iPar++) {
973 if (_params[iPar-1]->type == bncParam::AMB_L3 &&
974 _params[iPar-1]->prn == satData->prn) {
975 found = true;
976 break;
977 }
978 }
979 if (!found) {
980 bncParam* par = new bncParam(bncParam::AMB_L3,
981 _params.size()+1, satData->prn);
982 _params.push_back(par);
983 par->xx = satData->L3 - cmpValue(satData, true);
984 }
985}
986
987//
988///////////////////////////////////////////////////////////////////////////
989void bncModel::addObs(int iPhase, unsigned& iObs, t_satData* satData,
990 Matrix& AA, ColumnVector& ll, DiagonalMatrix& PP) {
991
992 Tracer tracer("bncModel::addObs");
993
994 const double ELEWGHT = 20.0;
995 double ellWgtCoef = 1.0;
996 double eleD = satData->eleSat * 180.0 / M_PI;
997 if (eleD < ELEWGHT) {
998 ellWgtCoef = 1.5 - 0.5 / (ELEWGHT - 10.0) * (eleD - 10.0);
999 }
1000
1001 // Remember Observation Index
1002 // --------------------------
1003 ++iObs;
1004 satData->obsIndex = iObs;
1005
1006 // Phase Observations
1007 // ------------------
1008 if (iPhase == 1) {
1009 ll(iObs) = satData->L3 - cmpValue(satData, true);
1010 double sigL3 = _opt->sigL3;
1011 if (satData->system() == 'R') {
1012 sigL3 *= GLONASS_WEIGHT_FACTOR;
1013 }
1014 PP(iObs,iObs) = 1.0 / (sigL3 * sigL3) / (ellWgtCoef * ellWgtCoef);
1015 for (int iPar = 1; iPar <= _params.size(); iPar++) {
1016 if (_params[iPar-1]->type == bncParam::AMB_L3 &&
1017 _params[iPar-1]->prn == satData->prn) {
1018 ll(iObs) -= _params[iPar-1]->xx;
1019 }
1020 AA(iObs, iPar) = _params[iPar-1]->partial(satData, true);
1021 }
1022 }
1023
1024 // Code Observations
1025 // -----------------
1026 else {
1027 ll(iObs) = satData->P3 - cmpValue(satData, false);
1028 PP(iObs,iObs) = 1.0 / (_opt->sigP3 * _opt->sigP3) / (ellWgtCoef * ellWgtCoef);
1029 for (int iPar = 1; iPar <= _params.size(); iPar++) {
1030 AA(iObs, iPar) = _params[iPar-1]->partial(satData, false);
1031 }
1032 }
1033}
1034
1035//
1036///////////////////////////////////////////////////////////////////////////
1037QByteArray bncModel::printRes(int iPhase, const ColumnVector& vv,
1038 const QMap<QString, t_satData*>& satDataMap) {
1039
1040 Tracer tracer("bncModel::printRes");
1041
1042 ostringstream str;
1043 str.setf(ios::fixed);
1044
1045 QMapIterator<QString, t_satData*> it(satDataMap);
1046 while (it.hasNext()) {
1047 it.next();
1048 t_satData* satData = it.value();
1049 if (satData->obsIndex != 0) {
1050 str << _time.timestr(1)
1051 << " RES " << satData->prn.toAscii().data()
1052 << (iPhase ? " L3 " : " P3 ")
1053 << setw(9) << setprecision(4) << vv(satData->obsIndex) << endl;
1054 }
1055 }
1056
1057 return QByteArray(str.str().c_str());
1058}
1059
1060//
1061///////////////////////////////////////////////////////////////////////////
1062void bncModel::findMaxRes(const ColumnVector& vv,
1063 const QMap<QString, t_satData*>& satData,
1064 QString& prnGPS, QString& prnGlo,
1065 double& maxResGPS, double& maxResGlo) {
1066
1067 Tracer tracer("bncModel::findMaxRes");
1068
1069 maxResGPS = 0.0;
1070 maxResGlo = 0.0;
1071
1072 QMapIterator<QString, t_satData*> it(satData);
1073 while (it.hasNext()) {
1074 it.next();
1075 t_satData* satData = it.value();
1076 if (satData->obsIndex != 0) {
1077 QString prn = satData->prn;
1078 if (prn[0] == 'R') {
1079 if (fabs(vv(satData->obsIndex)) > maxResGlo) {
1080 maxResGlo = fabs(vv(satData->obsIndex));
1081 prnGlo = prn;
1082 }
1083 }
1084 else {
1085 if (fabs(vv(satData->obsIndex)) > maxResGPS) {
1086 maxResGPS = fabs(vv(satData->obsIndex));
1087 prnGPS = prn;
1088 }
1089 }
1090 }
1091 }
1092}
1093
1094// Update Step (private - loop over outliers)
1095////////////////////////////////////////////////////////////////////////////
1096t_irc bncModel::update_p(t_epoData* epoData) {
1097
1098 Tracer tracer("bncModel::update_p");
1099
1100 // Save Variance-Covariance Matrix, and Status Vector
1101 // --------------------------------------------------
1102 rememberState(epoData);
1103
1104 QString lastOutlierPrn;
1105
1106 // Try with all satellites, then with all minus one, etc.
1107 // ------------------------------------------------------
1108 while (selectSatellites(lastOutlierPrn, epoData->satData) == success) {
1109
1110 QByteArray strResCode;
1111 QByteArray strResPhase;
1112
1113 // Bancroft Solution
1114 // -----------------
1115 if (cmpBancroft(epoData) != success) {
1116 break;
1117 }
1118
1119 // First update using code observations, then phase observations
1120 // -------------------------------------------------------------
1121 for (int iPhase = 0; iPhase <= (_opt->usePhase ? 1 : 0); iPhase++) {
1122
1123 // Status Prediction
1124 // -----------------
1125 predict(iPhase, epoData);
1126
1127 // Create First-Design Matrix
1128 // --------------------------
1129 unsigned nPar = _params.size();
1130 unsigned nObs = 0;
1131 if (iPhase == 0) {
1132 nObs = epoData->sizeAll() - epoData->sizeSys('R'); // Glonass code not used
1133 }
1134 else {
1135 nObs = epoData->sizeAll();
1136 }
1137
1138 // Prepare first-design Matrix, vector observed-computed
1139 // -----------------------------------------------------
1140 Matrix AA(nObs, nPar); // first design matrix
1141 ColumnVector ll(nObs); // tems observed-computed
1142 DiagonalMatrix PP(nObs); PP = 0.0;
1143
1144 unsigned iObs = 0;
1145 QMapIterator<QString, t_satData*> it(epoData->satData);
1146 while (it.hasNext()) {
1147 it.next();
1148 t_satData* satData = it.value();
1149 if (iPhase == 1 || satData->system() != 'R') {
1150 QString prn = satData->prn;
1151 addObs(iPhase, iObs, satData, AA, ll, PP);
1152 }
1153 }
1154
1155 // Compute Filter Update
1156 // ---------------------
1157 ColumnVector dx;
1158 kalman(AA, ll, PP, _QQ, dx);
1159 ColumnVector vv = ll - AA * dx;
1160
1161 // Print Residuals
1162 // ---------------
1163 if (iPhase == 0) {
1164 strResCode = printRes(iPhase, vv, epoData->satData);
1165 }
1166 else {
1167 strResPhase = printRes(iPhase, vv, epoData->satData);
1168 }
1169
1170 // Check the residuals
1171 // -------------------
1172 lastOutlierPrn = outlierDetection(iPhase, vv, epoData->satData);
1173
1174 // No Outlier Detected
1175 // -------------------
1176 if (lastOutlierPrn.isEmpty()) {
1177
1178 QVectorIterator<bncParam*> itPar(_params);
1179 while (itPar.hasNext()) {
1180 bncParam* par = itPar.next();
1181 par->xx += dx(par->index);
1182 }
1183
1184 if (!_opt->usePhase || iPhase == 1) {
1185 if (_outlierGPS.size() > 0 || _outlierGlo.size() > 0) {
1186 _log += "Neglected PRNs: ";
1187 if (!_outlierGPS.isEmpty()) {
1188 _log += _outlierGPS.last() + ' ';
1189 }
1190 QStringListIterator itGlo(_outlierGlo);
1191 while (itGlo.hasNext()) {
1192 QString prn = itGlo.next();
1193 _log += prn + ' ';
1194 }
1195 }
1196 _log += '\n';
1197
1198 _log += strResCode + strResPhase;
1199
1200 return success;
1201 }
1202 }
1203
1204 // Outlier Found
1205 // -------------
1206 else {
1207 restoreState(epoData);
1208 break;
1209 }
1210
1211 } // for iPhase
1212
1213 } // while selectSatellites
1214
1215 restoreState(epoData);
1216 return failure;
1217}
1218
1219// Remeber Original State Vector and Variance-Covariance Matrix
1220////////////////////////////////////////////////////////////////////////////
1221void bncModel::rememberState(t_epoData* epoData) {
1222
1223 _QQ_sav = _QQ;
1224
1225 QVectorIterator<bncParam*> itSav(_params_sav);
1226 while (itSav.hasNext()) {
1227 bncParam* par = itSav.next();
1228 delete par;
1229 }
1230 _params_sav.clear();
1231
1232 QVectorIterator<bncParam*> it(_params);
1233 while (it.hasNext()) {
1234 bncParam* par = it.next();
1235 _params_sav.push_back(new bncParam(*par));
1236 }
1237
1238 _epoData_sav->deepCopy(epoData);
1239}
1240
1241// Restore Original State Vector and Variance-Covariance Matrix
1242////////////////////////////////////////////////////////////////////////////
1243void bncModel::restoreState(t_epoData* epoData) {
1244
1245 _QQ = _QQ_sav;
1246
1247 QVectorIterator<bncParam*> it(_params);
1248 while (it.hasNext()) {
1249 bncParam* par = it.next();
1250 delete par;
1251 }
1252 _params.clear();
1253
1254 QVectorIterator<bncParam*> itSav(_params_sav);
1255 while (itSav.hasNext()) {
1256 bncParam* par = itSav.next();
1257 _params.push_back(new bncParam(*par));
1258 }
1259
1260 epoData->deepCopy(_epoData_sav);
1261}
1262
1263//
1264////////////////////////////////////////////////////////////////////////////
1265t_irc bncModel::selectSatellites(const QString& lastOutlierPrn,
1266 QMap<QString, t_satData*>& satData) {
1267
1268 // First Call
1269 // ----------
1270 if (lastOutlierPrn.isEmpty()) {
1271 _outlierGPS.clear();
1272 _outlierGlo.clear();
1273 return success;
1274 }
1275
1276 // Second and next trials
1277 // ----------------------
1278 else {
1279
1280 if (lastOutlierPrn[0] == 'R') {
1281 _outlierGlo << lastOutlierPrn;
1282 }
1283
1284 // Remove all Glonass Outliers
1285 // ---------------------------
1286 QStringListIterator it(_outlierGlo);
1287 while (it.hasNext()) {
1288 QString prn = it.next();
1289 if (satData.contains(prn)) {
1290 delete satData.take(prn);
1291 }
1292 }
1293
1294 if (lastOutlierPrn[0] == 'R') {
1295 _outlierGPS.clear();
1296 return success;
1297 }
1298
1299 // GPS Outlier appeared for the first time - try to delete it
1300 // ----------------------------------------------------------
1301 if (_outlierGPS.indexOf(lastOutlierPrn) == -1) {
1302 _outlierGPS << lastOutlierPrn;
1303 if (satData.contains(lastOutlierPrn)) {
1304 delete satData.take(lastOutlierPrn);
1305 }
1306 return success;
1307 }
1308
1309 }
1310
1311 return failure;
1312}
Note: See TracBrowser for help on using the repository browser.