source: ntrip/trunk/BNC/src/PPP_free/bncmodel.cpp@ 6086

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