source: ntrip/trunk/BNC/src/PPP/pppFilter.cpp@ 9656

Last change on this file since 9656 was 9656, checked in by stuerze, 2 years ago

minor changes regarding PPP
  • Property svn:keywords set to Author Date Id Rev URL;svn:eol-style=native
  • Property svn:mime-type set to text/plain
File size: 27.7 KB
Line 
1/* -------------------------------------------------------------------------
2 * BKG NTRIP Client
3 * -------------------------------------------------------------------------
4 *
5 * Class: t_pppFilter
6 *
7 * Purpose: Filter Adjustment
8 *
9 * Author: L. Mervart
10 *
11 * Created: 29-Jul-2014
12 *
13 * Changes:
14 *
15 * -----------------------------------------------------------------------*/
16
17#include <iostream>
18#include <iomanip>
19#include <cmath>
20#include <newmat.h>
21#include <newmatio.h>
22#include <newmatap.h>
23
24#include "pppFilter.h"
25#include "bncutils.h"
26#include "pppParlist.h"
27#include "pppObsPool.h"
28#include "pppStation.h"
29
30using namespace BNC_PPP;
31using namespace std;
32
33// Constructor
34////////////////////////////////////////////////////////////////////////////
35t_pppFilter::t_pppFilter(t_pppObsPool *obsPool) {
36 _numSat = 0;
37 _obsPool = obsPool;
38 _refPrn = t_prn();
39 _datumTrafo = new t_datumTrafo();
40}
41
42// Destructor
43////////////////////////////////////////////////////////////////////////////
44t_pppFilter::~t_pppFilter() {
45 delete _datumTrafo;
46}
47
48// Process Single Epoch
49////////////////////////////////////////////////////////////////////////////
50t_irc t_pppFilter::processEpoch() {
51 _numSat = 0;
52 const double maxSolGap = 60.0;
53
54 // Vector of all Observations
55 // --------------------------
56 t_pppObsPool::t_epoch *epoch = _obsPool->lastEpoch();
57 if (!epoch) {
58 return failure;
59 }
60 vector<t_pppSatObs*> &allObs = epoch->obsVector();
61
62 // Time of the Epoch
63 // -----------------
64 _epoTime = epoch->epoTime();
65
66 if (!_firstEpoTime.valid() || !_lastEpoTimeOK.valid()
67 || (maxSolGap > 0.0 && _epoTime - _lastEpoTimeOK > maxSolGap)) {
68 _firstEpoTime = _epoTime;
69 }
70
71 string epoTimeStr = string(_epoTime);
72
73 const QMap<char, t_pppRefSat*> &refSatMap = epoch->refSatMap();
74
75 //LOG << "processEpoch: printParams before set" << endl;
76 //_parlist.printParams(_epoTime);
77
78 //--
79 // Set Parameters
80 if (_parlist.set(_epoTime, allObs, refSatMap) != success) {
81 return failure;
82 }
83
84 if (OPT->_obsModelType == OPT->DCMcodeBias ||
85 OPT->_obsModelType == OPT->DCMphaseBias) {
86#ifdef BNC_DEBUG_PPP
87 _parlist.printParams(_epoTime);
88#endif
89 }
90 // Status Vector, Variance-Covariance Matrix
91 // -----------------------------------------
92 ColumnVector xFltOld = _xFlt;
93 SymmetricMatrix QFltOld = _QFlt;
94 setStateVectorAndVarCovMatrix(xFltOld, QFltOld);
95
96 // Pre-Process Satellite Systems separately
97 // ----------------------------------------
98 bool preProcessing = false;
99 if (OPT->_obsModelType == OPT->DCMcodeBias ||
100 OPT->_obsModelType == OPT->DCMphaseBias) {
101 preProcessing = true;
102 const QList<char> &usedSystems = _parlist.usedSystems();
103 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
104 char sys = usedSystems[iSys];
105 _refPrn.set(sys, 0);
106 if (OPT->_refSatRequired) {
107 _refPrn = refSatMap[sys]->prn();
108 }
109 vector<t_pppSatObs*> obsVector;
110 for (unsigned jj = 0; jj < allObs.size(); jj++) {
111 if (allObs[jj]->prn().system() == sys) {
112 obsVector.push_back(allObs[jj]);
113 }
114 }
115 if (iSys == 0) {
116 _datumTrafo->setFirstSystem(sys);
117 }
118 if (processSystem(OPT->LCs(sys), obsVector, _refPrn,
119 epoch->pseudoObsIono(), preProcessing) != success) {
120 LOG << sys << ": processSystem != success (pre-processing)" << endl;
121 _xFlt = xFltOld;
122 _QFlt = QFltOld;
123 _obsPool->deleteLastEpoch();
124 restoreState(2);
125 return failure;
126 }
127 }
128 // refSat change required?
129 // -----------------------
130 if (_obsPool->refSatChangeRequired()) {
131 _xFlt = xFltOld;
132 _QFlt = QFltOld;
133 return success;
134 } else if (!_obsPool->refSatChangeRequired()) {
135 initDatumTransformation(allObs, epoch->pseudoObsIono());
136 }
137 }
138
139 // Process Satellite Systems separately
140 // ------------------------------------
141 preProcessing = false;
142 const QList<char> &usedSystems = _parlist.usedSystems();
143 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
144 char sys = usedSystems[iSys];
145 _refPrn.set(sys, 0);
146 if (OPT->_refSatRequired) {
147 _refPrn = refSatMap[sys]->prn();
148 }
149 unsigned int num = 0;
150 vector<t_pppSatObs*> obsVector;
151 for (unsigned jj = 0; jj < allObs.size(); jj++) {
152 if (allObs[jj]->prn().system() == sys) {
153 obsVector.push_back(allObs[jj]);
154 ++num;
155 }
156 }
157 if (iSys == 0 && OPT->_obsModelType == OPT->UncombPPP) {
158 _datumTrafo->setFirstSystem(sys);
159 }
160 LOG << epoTimeStr << " SATNUM " << sys << ' ' << right << setw(2) << num
161 << endl;
162 if (processSystem(OPT->LCs(sys), obsVector, _refPrn, epoch->pseudoObsIono(),
163 preProcessing) != success) {
164 LOG << "processSystem != success (fin-processing)" << endl;
165 if (OPT->_obsModelType == OPT->DCMcodeBias ||
166 OPT->_obsModelType == OPT->DCMphaseBias) {
167 _xFlt = xFltOld;
168 _QFlt = QFltOld;
169 _obsPool->deleteLastEpoch();
170 restoreState(3);
171 }
172 return failure;
173 }
174 }
175
176 // close epoch processing
177 // ----------------------
178 cmpDOP(allObs, refSatMap);
179 _parlist.printResult(_epoTime, _QFlt, _xFlt);
180 _lastEpoTimeOK = _epoTime; // remember time of last successful epoch processing
181 return success;
182}
183
184// Process Selected LCs
185////////////////////////////////////////////////////////////////////////////
186t_irc t_pppFilter::processSystem(const vector<t_lc::type> &LCs,
187 const vector<t_pppSatObs*> &obsVector, const t_prn &refPrn,
188 bool pseudoObsIonoAvailable, bool preProcessing) {
189 LOG.setf(ios::fixed);
190 char sys = refPrn.system();
191
192 // Detect Cycle Slips
193 // ------------------
194 if (detectCycleSlips(LCs, obsVector, refPrn, preProcessing) != success) {
195 return failure;
196 }
197 if (preProcessing && _obsPool->refSatChangeRequired(sys)) {
198 return success;
199 }
200
201 ColumnVector xSav = _xFlt;
202 SymmetricMatrix QSav = _QFlt;
203 string epoTimeStr = string(_epoTime);
204 const vector<t_pppParam*> &params = _parlist.params();
205 unsigned nPar = _parlist.nPar();
206
207 unsigned usedLCs = LCs.size();
208 if (OPT->_pseudoObsIono && !pseudoObsIonoAvailable) {
209 usedLCs -= 1; // GIM not used
210 }
211 // max Obs
212 unsigned maxObs = obsVector.size() * usedLCs;
213
214 if (OPT->_pseudoObsIono && pseudoObsIonoAvailable) {
215 maxObs -= 1; // pseudo obs iono with respect to refSat
216 }
217
218 // Outlier Detection Loop
219 // ----------------------
220 for (unsigned iOutlier = 0; iOutlier < maxObs; iOutlier++) {
221
222 if (iOutlier > 0) {
223 _xFlt = xSav;
224 _QFlt = QSav;
225 }
226
227 // First-Design Matrix, Terms Observed-Computed, Weight Matrix
228 // -----------------------------------------------------------
229 Matrix AA(maxObs, nPar);
230 ColumnVector ll(maxObs);
231 DiagonalMatrix PP(maxObs);
232 PP = 0.0;
233
234 int iObs = -1;
235 vector<t_pppSatObs*> usedObs;
236 vector<t_lc::type> usedTypes;
237
238 // Real Observations
239 // =================
240 double nSat = 0;
241 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
242 t_pppSatObs *obs = obsVector[ii];
243 if (iOutlier == 0 && !preProcessing) {
244 obs->resetOutlier();
245 }
246 if (!obs->outlier()) {
247 nSat++;
248 for (unsigned jj = 0; jj < usedLCs; jj++) {
249 const t_lc::type tLC = LCs[jj];
250 if (tLC == t_lc::GIM) {
251 continue;
252 }
253 ++iObs;
254 usedObs.push_back(obs);
255 usedTypes.push_back(tLC);
256 for (unsigned iPar = 0; iPar < nPar; iPar++) {
257 const t_pppParam *par = params[iPar];
258 AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
259 }
260 ll[iObs] = obs->obsValue(tLC) - obs->cmpValue(tLC)
261 - DotProduct(_x0, AA.Row(iObs + 1));
262 PP[iObs] = 1.0 / (obs->sigma(tLC) * obs->sigma(tLC));
263 }
264 }
265 }
266
267 // Check number of observations
268 // ----------------------------
269 if (iObs == -1) {
270 LOG << " number of observations == -1\n";
271 return failure;
272 }
273
274 if ((!iOutlier) && (OPT->_obsModelType == OPT->DCMcodeBias ||
275 OPT->_obsModelType == OPT->DCMphaseBias) && (!preProcessing)) {
276 _datumTrafo->updateIndices(sys, iObs + 1);
277 _datumTrafo->prepareAA(AA.SubMatrix(1, iObs + 1, 1, _parlist.nPar()), 1);
278 }
279
280 // Pseudo Obs Iono
281 // ================
282 if (OPT->_pseudoObsIono && pseudoObsIonoAvailable) {
283 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
284 t_pppSatObs *obs = obsVector[ii];
285 if (!obs->outlier()) {
286 for (unsigned jj = 0; jj < usedLCs; jj++) {
287 const t_lc::type tLC = LCs[jj];
288 if (tLC == t_lc::GIM && !obs->isReference()) {
289 ++iObs;
290 } else {
291 continue;
292 }
293 usedObs.push_back(obs);
294 usedTypes.push_back(tLC);
295 for (unsigned iPar = 0; iPar < nPar; iPar++) {
296 const t_pppParam *par = params[iPar];
297 AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
298 }
299 ll[iObs] = obs->obsValue(tLC) - obs->cmpValue(tLC)
300 - DotProduct(_x0, AA.Row(iObs + 1));
301 PP[iObs] = 1.0 / (obs->sigma(tLC) * obs->sigma(tLC));
302 }
303 }
304 }
305 }
306
307 // Truncate matrices
308 // -----------------
309 AA = AA.Rows(1, iObs + 1);
310 ll = ll.Rows(1, iObs + 1);
311 PP = PP.SymSubMatrix(1, iObs + 1);
312
313 // Kalman update step
314 // ------------------
315 kalman(AA, ll, PP, _QFlt, _xFlt);
316
317 // Check Residuals
318 // ---------------
319 ColumnVector vv = AA * _xFlt - ll;
320 double maxOutlier = 0.0;
321 int maxOutlierIndex = -1;
322 t_lc::type maxOutlierLC = t_lc::dummy;
323 for (unsigned ii = 0; ii < usedObs.size(); ii++) {
324 const t_lc::type tLC = usedTypes[ii];
325 double res = fabs(vv[ii]);
326 if (res > usedObs[ii]->maxRes(tLC)) {
327 if (res > fabs(maxOutlier)) {
328 maxOutlier = vv[ii];
329 maxOutlierIndex = ii;
330 maxOutlierLC = tLC;
331 }
332 }
333 }
334
335 // Mark outlier or break outlier detection loop
336 // --------------------------------------------
337 if (maxOutlierIndex > -1) {
338 t_pppSatObs *obs = usedObs[maxOutlierIndex];
339 t_pppParam *par = 0;
340 for (unsigned iPar = 0; iPar < nPar; iPar++) {
341 t_pppParam *hlp = params[iPar];
342 if (hlp->type() == t_pppParam::amb && hlp->prn() == obs->prn()
343 && hlp->tLC() == usedTypes[maxOutlierIndex]) {
344 par = hlp;
345 }
346 }
347 if (preProcessing) {
348 // for refSats no ambiguity parameter exists
349 if ((obs->prn() == refPrn)
350 && (t_lc::toString(maxOutlierLC) == "l1"
351 || t_lc::toString(maxOutlierLC) == "l2")) {
352 _obsPool->setRefSatChangeRequired(sys, true);
353 LOG << epoTimeStr << " Outlier ("
354 << ((preProcessing) ? "pre-processing) " : "fin-processing) ")
355 << t_lc::toString(maxOutlierLC) << ' ' << obs->prn().toString()
356 << ' ' << setw(8) << setprecision(4) << maxOutlier << endl;
357 break;
358 } else {
359 obs->setOutlier();
360 }
361 } else { // fin-processing
362 LOG << epoTimeStr << " Outlier " << t_lc::toString(maxOutlierLC) << ' '
363 << obs->prn().toString() << ' ' << setw(8) << setprecision(4)
364 << maxOutlier << endl;
365 if (par) {
366 resetAmb(par->prn(), obsVector, &QSav, &xSav);
367 } else {
368 obs->setOutlier();
369 }
370 }
371 }
372 // Print Residuals
373 // ---------------
374 else {
375 if (!preProcessing) {
376 for (unsigned jj = 0; jj < LCs.size(); jj++) {
377 for (unsigned ii = 0; ii < usedObs.size(); ii++) {
378 const t_lc::type tLC = usedTypes[ii];
379 t_pppSatObs *obs = usedObs[ii];
380 if (tLC == LCs[jj]) {
381 obs->setRes(tLC, vv[ii]);
382 LOG << epoTimeStr << " RES " << left << setw(3)
383 << t_lc::toString(tLC) << right << ' ';
384 if (t_lc::toString(tLC) == "Tz0") {
385 LOG << sys << " ";
386 } else {
387 LOG << obs->prn().toString();
388 }
389 LOG << setw(9) << setprecision(4) << vv[ii] << endl;
390 }
391 }
392 }
393 }
394 break;
395 }
396 }
397 return success;
398}
399
400// Cycle-Slip Detection
401////////////////////////////////////////////////////////////////////////////
402t_irc t_pppFilter::detectCycleSlips(const vector<t_lc::type> &LCs,
403 const vector<t_pppSatObs*> &obsVector, const t_prn &refPrn,
404 bool preProcessing) {
405 const double SLIP = 20.0;
406 char sys = refPrn.system();
407 string epoTimeStr = string(_epoTime);
408 const vector<t_pppParam*> &params = _parlist.params();
409 unsigned nPar = _parlist.nPar();
410
411 for (unsigned ii = 0; ii < LCs.size(); ii++) {
412 const t_lc::type &tLC = LCs[ii];
413 if (t_lc::includesPhase(tLC)) {
414 for (unsigned iObs = 0; iObs < obsVector.size(); iObs++) {
415 const t_pppSatObs *obs = obsVector[iObs];
416
417 // Check set Slips and Jump Counters
418 // ---------------------------------
419 bool slip = false;
420
421 // in pre-processing only the reference satellite has to be checked
422 if (preProcessing && obs->prn() != refPrn) {
423 continue;
424 }
425
426 if (obs->slip()) {
427 LOG << epoTimeStr << "cycle slip set (obs) " << obs->prn().toString()
428 << endl;
429 slip = true;
430 }
431
432 if (_slips[obs->prn()]._obsSlipCounter != -1
433 && _slips[obs->prn()]._obsSlipCounter != obs->slipCounter()) {
434 LOG << epoTimeStr << "cycle slip set (obsSlipCounter) "
435 << obs->prn().toString() << endl;
436 slip = true;
437 }
438 if (!preProcessing) {
439 _slips[obs->prn()]._obsSlipCounter = obs->slipCounter();
440 }
441 if (_slips[obs->prn()]._biasJumpCounter != -1
442 && _slips[obs->prn()]._biasJumpCounter != obs->biasJumpCounter()) {
443 LOG << epoTimeStr << "cycle slip set (biasJumpCounter) "
444 << obs->prn().toString() << endl;
445 slip = true;
446 }
447 if (!preProcessing) {
448 _slips[obs->prn()]._biasJumpCounter = obs->biasJumpCounter();
449 }
450 // Slip Set
451 // --------
452 if (slip) {
453 if (preProcessing) {
454 _obsPool->setRefSatChangeRequired(sys, true);
455 } else {
456 resetAmb(obs->prn(), obsVector);
457 }
458 }
459 // Check Pre-Fit Residuals
460 // -----------------------
461 else {
462 if (refPrn != t_prn()) {
463 return success;
464 }
465 ColumnVector AA(nPar);
466 for (unsigned iPar = 0; iPar < nPar; iPar++) {
467 const t_pppParam *par = params[iPar];
468 AA[iPar] = par->partial(_epoTime, obs, tLC, refPrn);
469 }
470 double ll = obs->obsValue(tLC) - obs->cmpValue(tLC)
471 - DotProduct(_x0, AA);
472 double vv = DotProduct(AA, _xFlt) - ll;
473 if (fabs(vv) > SLIP) {
474 LOG << epoTimeStr << " cycle slip detected " << t_lc::toString(tLC)
475 << ' ' << obs->prn().toString() << ' ' << setw(8)
476 << setprecision(4) << vv << endl;
477 if (preProcessing) {
478 _obsPool->setRefSatChangeRequired(sys, true);
479 } else {
480 resetAmb(obs->prn(), obsVector);
481 }
482 }
483 }
484 }
485 }
486 }
487 return success;
488}
489
490// Reset Ambiguity Parameter (cycle slip)
491////////////////////////////////////////////////////////////////////////////
492t_irc t_pppFilter::resetAmb(t_prn prn, const vector<t_pppSatObs*> &obsVector,
493 SymmetricMatrix *QSav, ColumnVector *xSav) {
494
495 t_irc irc = failure;
496 vector<t_pppParam*> &params = _parlist.params();
497 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
498 t_pppParam *par = params[iPar];
499 if (par->type() == t_pppParam::amb && par->prn() == prn) {
500 int ind = par->indexNew();
501 t_lc::type tLC = par->tLC();
502 LOG << string(_epoTime) << " RESET " << par->toString() << endl;
503 delete par;
504 par = new t_pppParam(t_pppParam::amb, prn, tLC, &obsVector);
505 par->setIndex(ind);
506 params[iPar] = par;
507 for (unsigned ii = 1; ii <= params.size(); ii++) {
508 _QFlt(ii, ind + 1) = 0.0;
509 if (QSav) {
510 (*QSav)(ii, ind + 1) = 0.0;
511 }
512 }
513 _QFlt(ind + 1, ind + 1) = par->sigma0() * par->sigma0();
514 if (QSav) {
515 (*QSav)(ind + 1, ind + 1) = _QFlt(ind + 1, ind + 1);
516 }
517 _xFlt[ind] = 0.0;
518 if (xSav) {
519 (*xSav)[ind] = _xFlt[ind];
520 }
521 _x0[ind] = par->x0();
522 irc = success;
523 }
524 }
525
526 return irc;
527}
528
529// Add infinite noise to iono
530////////////////////////////////////////////////////////////////////////////
531t_irc t_pppFilter::addNoiseToPar(t_pppParam::e_type parType, char sys) {
532
533 t_irc irc = failure;
534 vector<t_pppParam*> &params = _parlist.params();
535 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
536 t_pppParam *par = params[iPar];
537 if (par->type() == parType && par->prn().system() == sys) {
538 int ind = par->indexNew();
539 LOG << string(_epoTime) << " ADD IONO_NOISE TO " << par->prn().toString()
540 << endl;
541 par->setIndex(ind);
542 _QFlt(ind + 1, ind + 1) = par->sigma0() * par->sigma0();
543 irc = success;
544 }
545 }
546
547 return irc;
548}
549
550// Compute various DOP Values
551////////////////////////////////////////////////////////////////////////////
552void t_pppFilter::cmpDOP(const vector<t_pppSatObs*> &obsVector,
553 const QMap<char, t_pppRefSat*> &refSatMap) {
554
555 _dop.reset();
556
557 try {
558 const unsigned numPar = 4;
559 Matrix AA(obsVector.size(), numPar);
560 _numSat = 0;
561 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
562 t_pppSatObs *obs = obsVector[ii];
563 char sys = obs->prn().system();
564 t_prn refPrn = t_prn();
565 if (OPT->_refSatRequired) {
566 refPrn = refSatMap[sys]->prn();
567 }
568 if (obs->isValid() && !obs->outlier()) {
569 ++_numSat;
570 for (unsigned iPar = 0; iPar < numPar; iPar++) {
571 const t_pppParam *par = _parlist.params()[iPar];
572 AA[_numSat - 1][iPar] = par->partial(_epoTime, obs, t_lc::c1, refPrn);
573 }
574 }
575 }
576 if (_numSat < 4) {
577 return;
578 }
579 AA = AA.Rows(1, _numSat);
580 SymmetricMatrix NN;
581 NN << AA.t() * AA;
582 SymmetricMatrix QQ = NN.i();
583
584 _dop.H = sqrt(QQ(1, 1) + QQ(2, 2));
585 _dop.V = sqrt(QQ(3, 3));
586 _dop.P = sqrt(QQ(1, 1) + QQ(2, 2) + QQ(3, 3));
587 _dop.T = sqrt(QQ(4, 4));
588 _dop.G = sqrt(QQ(1, 1) + QQ(2, 2) + QQ(3, 3) + QQ(4, 4));
589 } catch (...) {
590 }
591}
592
593//
594////////////////////////////////////////////////////////////////////////////
595void t_pppFilter::predictCovCrdPart(const SymmetricMatrix &QFltOld) {
596
597 const vector<t_pppParam*> &params = _parlist.params();
598 if (params.size() < 3) {
599 return;
600 }
601
602 bool first = (params[0]->indexOld() < 0);
603
604 SymmetricMatrix Qneu(3);
605 Qneu = 0.0;
606 for (unsigned ii = 0; ii < 3; ii++) {
607 const t_pppParam *par = params[ii];
608 if (first) {
609 Qneu[ii][ii] = par->sigma0() * par->sigma0();
610 } else {
611 Qneu[ii][ii] = par->noise() * par->noise();
612 }
613 }
614
615 const t_pppStation *sta = PPP_CLIENT->staRover();
616 SymmetricMatrix Qxyz(3);
617 covariNEU_XYZ(Qneu, sta->ellApr().data(), Qxyz);
618
619 if (first) {
620 _QFlt.SymSubMatrix(1, 3) = Qxyz;
621 } else {
622 double dt = _epoTime - _firstEpoTime;
623 if (dt < OPT->_seedingTime) {
624 _QFlt.SymSubMatrix(1, 3) = QFltOld.SymSubMatrix(1, 3);
625 } else {
626 _QFlt.SymSubMatrix(1, 3) = QFltOld.SymSubMatrix(1, 3) + Qxyz;
627 }
628 }
629}
630
631//
632////////////////////////////////////////////////////////////////////////////
633void t_pppFilter::setStateVectorAndVarCovMatrix(const ColumnVector &xFltOld,
634 const SymmetricMatrix &QFltOld) {
635
636 const vector<t_pppParam*> &params = _parlist.params();
637 unsigned nPar = params.size();
638
639 _QFlt.ReSize(nPar);
640 _QFlt = 0.0;
641 _xFlt.ReSize(nPar);
642 _xFlt = 0.0;
643 _x0.ReSize(nPar);
644 _x0 = 0.0;
645
646 for (unsigned ii = 0; ii < nPar; ii++) {
647 t_pppParam *par1 = params[ii];
648 if (QFltOld.size() == 0) {
649 par1->resetIndex();
650 }
651 _x0[ii] = par1->x0();
652 int iOld = par1->indexOld();
653 if (iOld < 0) {
654 _QFlt[ii][ii] = par1->sigma0() * par1->sigma0(); // new parameter
655 } else {
656 _QFlt[ii][ii] = QFltOld[iOld][iOld] + par1->noise() * par1->noise();
657 _xFlt[ii] = xFltOld[iOld];
658 for (unsigned jj = 0; jj < ii; jj++) {
659 t_pppParam *par2 = params[jj];
660 int jOld = par2->indexOld();
661 if (jOld >= 0) {
662 _QFlt[ii][jj] = QFltOld(iOld + 1, jOld + 1);
663 }
664 }
665 }
666 }
667 predictCovCrdPart(QFltOld);
668}
669
670// Compute datum transformation
671////////////////////////////////////////////////////////////////////////////
672t_irc t_pppFilter::datumTransformation(
673 const QMap<char, t_pppRefSat*> &refSatMap) {
674
675 // get last epoch
676 t_pppObsPool::t_epoch *epoch = _obsPool->lastEpoch();
677 if (!epoch) {
678 LOG << "t_pppFilter::datumTransformation: !lastEpoch" << endl;
679 return failure;
680 }
681 _epoTime = epoch->epoTime();
682 LOG.setf(ios::fixed);
683 LOG << string(_epoTime) << " DATUM TRANSFORMATION " << endl;
684
685 vector<t_pppSatObs*> &allObs = epoch->obsVector();
686
687 const QMap<char, t_pppRefSat*> &refSatMapLastEpoch = epoch->refSatMap();
688
689 bool pseudoObsIono = epoch->pseudoObsIono();
690
691 // reset old and set new refSats in last epoch (ambiguities/GIM)
692 // =============================================================
693 if (resetRefSatellitesLastEpoch(allObs, refSatMap, refSatMapLastEpoch)
694 != true) {
695 LOG << "datumTransformation: resetRefSatellitesLastEpoch != success"
696 << endl;
697 return failure;
698 }
699
700 if (OPT->_obsModelType == OPT->UncombPPP) {
701 _obsPool->putEpoch(_epoTime, allObs, pseudoObsIono, refSatMap);
702 return success;
703 }
704 //LOG << "datumTransformation: printParams before set" << endl;
705 //_parlist.printParams(_epoTime);
706
707 // set AA2
708 // =======
709 if (_parlist.set(_epoTime, allObs, refSatMap) != success) {
710 return failure;
711 }
712
713#ifdef BNC_DEBUG_PPP
714 _parlist.printParams(_epoTime);
715#endif
716
717 const QList<char> &usedSystems = _parlist.usedSystems();
718 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
719 char sys = usedSystems[iSys];
720 t_prn refPrn = refSatMap[sys]->prn();
721 vector<t_pppSatObs*> obsVector;
722 for (unsigned jj = 0; jj < allObs.size(); jj++) {
723 if (allObs[jj]->prn().system() == sys) {
724 allObs[jj]->resetOutlier();
725 obsVector.push_back(allObs[jj]);
726 }
727 }
728 if (iSys == 0) {
729 _datumTrafo->setFirstSystem(sys);
730 }
731 vector<t_lc::type> LCs = OPT->LCs(sys);
732 unsigned usedLCs = LCs.size();
733 if (OPT->_pseudoObsIono && !pseudoObsIono) {
734 usedLCs -= 1; // GIM not used
735 }
736 // max Obs
737 unsigned maxObs = obsVector.size() * usedLCs;
738
739 if (OPT->_pseudoObsIono && pseudoObsIono) {
740 maxObs -= 1; // pseudo obs iono with respect to refSat
741 }
742
743 const vector<t_pppParam*> &params = _parlist.params();
744 unsigned nPar = _parlist.nPar();
745
746 Matrix AA(maxObs, nPar);
747
748 // Real Observations
749 // -----------------
750 int iObs = -1;
751 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
752 t_pppSatObs *obs = obsVector[ii];
753 for (unsigned jj = 0; jj < usedLCs; jj++) {
754 const t_lc::type tLC = LCs[jj];
755 if (tLC == t_lc::GIM) {
756 continue;
757 }
758 ++iObs;
759 for (unsigned iPar = 0; iPar < nPar; iPar++) {
760 const t_pppParam *par = params[iPar];
761 AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
762 }
763 }
764 }
765
766 if (!iObs) {
767 continue;
768 }
769 _datumTrafo->updateIndices(sys, iObs + 1); //LOG << "AA Ncols/Nrows: " << AA.Ncols() << "/" << AA.Nrows() << " nPar: " << nPar << endl; //LOG << "AA.SubMatrix(1 .. " << iObs+1 << " , 1 .. " << nPar << ")" << endl;
770 if (_datumTrafo->prepareAA(AA.SubMatrix(1, iObs + 1, 1, nPar), 2)
771 != success) {
772 return failure;
773 }
774 }
775 _datumTrafo->updateNumObs();
776
777 // Datum Transformation
778 // ====================
779#ifdef BNC_DEBUG_PPP
780 //LOG << "AA1\n"; _datumTrafo->printMatrix(_datumTrafo->AA1(), _datumTrafo->numObs(), _datumTrafo->numPar());
781 //LOG << "AA2\n"; _datumTrafo->printMatrix(_datumTrafo->AA2(), _datumTrafo->numObs(), _datumTrafo->numPar());
782#endif
783 if (_datumTrafo->computeTrafoMatrix() != success) {
784 return failure;
785 }
786#ifdef BNC_DEBUG_PPP
787 //LOG << "D21" << endl; _datumTrafo->printMatrix(_datumTrafo->D21(), _datumTrafo->numObs(), _datumTrafo->numPar());
788#endif
789 ColumnVector xFltOld = _xFlt;
790 SymmetricMatrix QFltOld = _QFlt;
791
792 _QFlt << _datumTrafo->D21() * QFltOld * _datumTrafo->D21().t();
793 _xFlt = _datumTrafo->D21() * xFltOld;
794
795#ifdef BNC_DEBUG_PPP
796 LOG << "xFltOld:\n" << xFltOld << endl;
797 LOG << "xFlt :\n" << _xFlt << endl;
798#endif
799
800 // Reset Ambiguities after Datum Transformation
801 // ============================================
802 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
803 char sys = usedSystems[iSys];
804 t_prn refPrnOld = refSatMapLastEpoch[sys]->prn();
805 t_prn refPrnNew = refSatMap[sys]->prn();
806 if (refPrnNew != refPrnOld) {
807 resetAmb(refPrnOld, allObs);
808 if (resetAmb(refPrnOld, allObs) == success) {
809 if (OPT->_obsModelType == OPT->DCMcodeBias) {
810 addNoiseToPar(t_pppParam::ion, sys);
811 } else if (OPT->_obsModelType == OPT->DCMphaseBias) {
812 if (sys == 'G') {
813 addNoiseToPar(t_pppParam::pBiasG1, sys);
814 addNoiseToPar(t_pppParam::pBiasG2, sys);
815 } else if (sys == 'R') {
816 addNoiseToPar(t_pppParam::pBiasR1, sys);
817 addNoiseToPar(t_pppParam::pBiasR2, sys);
818 } else if (sys == 'E') {
819 addNoiseToPar(t_pppParam::pBiasE1, sys);
820 addNoiseToPar(t_pppParam::pBiasE2, sys);
821 } else if (sys == 'C') {
822 addNoiseToPar(t_pppParam::pBiasC1, sys);
823 addNoiseToPar(t_pppParam::pBiasC2, sys);
824 }
825 }
826 }
827 }
828 }
829
830 // switch AA2 to AA1
831 // =================
832 _datumTrafo->switchAA();
833
834 _obsPool->putEpoch(_epoTime, allObs, pseudoObsIono, refSatMap);
835
836 return success;
837}
838
839// Init datum transformation
840////////////////////////////////////////////////////////////////////////////
841void t_pppFilter::initDatumTransformation(
842 const std::vector<t_pppSatObs*> &allObs, bool pseudoObsIono) {
843 unsigned trafoObs = 0;
844 const QList<char> &usedSystems = _parlist.usedSystems();
845 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
846 char sys = usedSystems[iSys];
847 int satNum = 0;
848 for (unsigned jj = 0; jj < allObs.size(); jj++) {
849 if (allObs[jj]->prn().system() == sys) {
850 satNum++;
851 }
852 }
853 // all LCs
854 unsigned realUsedLCs = OPT->LCs(sys).size();
855 // exclude pseudo obs GIM
856 if (OPT->_pseudoObsIono && !pseudoObsIono) {
857 realUsedLCs -= 1;
858 }
859
860 trafoObs += satNum * realUsedLCs;
861
862 if (OPT->_pseudoObsIono && pseudoObsIono) {
863 trafoObs -= 1; // pseudo obs iono with respect to refSat
864 }
865 }
866 _datumTrafo->setNumObs(trafoObs);
867 _datumTrafo->setNumPar(_parlist.nPar());
868 _datumTrafo->initAA();
869}
870
871//
872//////////////////////////////////////////////////////////////////////////////
873bool t_pppFilter::resetRefSatellitesLastEpoch(
874 std::vector<t_pppSatObs*> &obsVector,
875 const QMap<char, t_pppRefSat*> &refSatMap,
876 const QMap<char, t_pppRefSat*> &refSatMapLastEpoch) {
877 bool resetRefSat;
878 // reference satellite definition per system
879 const QList<char> &usedSystems = _parlist.usedSystems();
880 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
881 resetRefSat = false;
882 char sys = usedSystems[iSys];
883 t_prn newPrn = refSatMap[sys]->prn();
884 t_prn oldPrn = refSatMapLastEpoch[sys]->prn();
885#ifdef BNC_DEBUG_PPP
886 if (oldPrn != newPrn) {
887 LOG << "oldRef: " << oldPrn.toString() << " => newRef " << newPrn.toString() << endl;
888 }
889#endif
890 vector<t_pppSatObs*>::iterator it = obsVector.begin();
891 while (it != obsVector.end()) {
892 t_pppSatObs *satObs = *it;
893 if (satObs->prn() == newPrn) {
894 resetRefSat = true;
895 satObs->setAsReference();
896 } else if (satObs->prn() == oldPrn) {
897 satObs->resetReference();
898 }
899 it++;
900 }
901 if (!resetRefSat) {
902 _obsPool->setRefSatChangeRequired(sys, true);
903 return resetRefSat;
904 }
905 }
906 return resetRefSat;
907}
Note: See TracBrowser for help on using the repository browser.