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

Last change on this file since 9704 was 9704, 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 //--
76 // Set Parameters
77 if (_parlist.set(_epoTime, allObs, refSatMap) != success) {
78 return failure;
79 }
80
81 if (OPT->_obsModelType == OPT->DCMcodeBias ||
82 OPT->_obsModelType == OPT->DCMphaseBias) {
83#ifdef BNC_DEBUG_PPP
84 _parlist.printParams(_epoTime);
85#endif
86 }
87 // Status Vector, Variance-Covariance Matrix
88 // -----------------------------------------
89 ColumnVector xFltOld = _xFlt;
90 SymmetricMatrix QFltOld = _QFlt;
91 setStateVectorAndVarCovMatrix(xFltOld, QFltOld);
92
93 // Pre-Process Satellite Systems separately
94 // ----------------------------------------
95 bool preProcessing = false;
96 if (OPT->_obsModelType == OPT->DCMcodeBias ||
97 OPT->_obsModelType == OPT->DCMphaseBias) {
98 preProcessing = true;
99 const QList<char> &usedSystems = _parlist.usedSystems();
100 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
101 char sys = usedSystems[iSys];
102 _refPrn.set(sys, 0);
103 if (OPT->_refSatRequired) {
104 _refPrn = refSatMap[sys]->prn();
105 }
106 vector<t_pppSatObs*> obsVector;
107 for (unsigned jj = 0; jj < allObs.size(); jj++) {
108 if (allObs[jj]->prn().system() == sys) {
109 obsVector.push_back(allObs[jj]);
110 }
111 }
112 if (iSys == 0) {
113 _datumTrafo->setFirstSystem(sys);
114 }
115 if (processSystem(OPT->LCs(sys), obsVector, _refPrn,
116 epoch->pseudoObsIono(), preProcessing) != success) {
117 LOG << sys << ": processSystem != success (pre-processing)" << endl;
118 _xFlt = xFltOld;
119 _QFlt = QFltOld;
120 _obsPool->deleteLastEpoch();
121 restoreState(2);
122 return failure;
123 }
124 }
125 // refSat change required?
126 // -----------------------
127 if (_obsPool->refSatChangeRequired()) {
128 _xFlt = xFltOld;
129 _QFlt = QFltOld;
130 return success;
131 } else if (!_obsPool->refSatChangeRequired()) {
132 initDatumTransformation(allObs, epoch->pseudoObsIono());
133 }
134 }
135
136 // Process Satellite Systems separately
137 // ------------------------------------
138 preProcessing = false;
139 const QList<char> &usedSystems = _parlist.usedSystems();
140 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
141 char sys = usedSystems[iSys];
142 _refPrn.set(sys, 0);
143 if (OPT->_refSatRequired) {
144 _refPrn = refSatMap[sys]->prn();
145 }
146 unsigned int num = 0;
147 vector<t_pppSatObs*> obsVector;
148 for (unsigned jj = 0; jj < allObs.size(); jj++) {
149 if (allObs[jj]->prn().system() == sys) {
150 obsVector.push_back(allObs[jj]);
151 ++num;
152 }
153 }
154 if (iSys == 0 && OPT->_obsModelType == OPT->UncombPPP) {
155 _datumTrafo->setFirstSystem(sys);
156 }
157 LOG << epoTimeStr << " SATNUM " << sys << ' ' << right << setw(2) << num
158 << endl;
159 if (processSystem(OPT->LCs(sys), obsVector, _refPrn, epoch->pseudoObsIono(),
160 preProcessing) != success) {
161 LOG << "processSystem != success (fin-processing)" << endl;
162 if (OPT->_obsModelType == OPT->DCMcodeBias ||
163 OPT->_obsModelType == OPT->DCMphaseBias) {
164 _xFlt = xFltOld;
165 _QFlt = QFltOld;
166 _obsPool->deleteLastEpoch();
167 restoreState(3);
168 }
169 return failure;
170 }
171 }
172
173 // close epoch processing
174 // ----------------------
175 cmpDOP(allObs, refSatMap);
176 _parlist.printResult(_epoTime, _QFlt, _xFlt);
177 _lastEpoTimeOK = _epoTime; // remember time of last successful epoch processing
178 return success;
179}
180
181// Process Selected LCs
182////////////////////////////////////////////////////////////////////////////
183t_irc t_pppFilter::processSystem(const vector<t_lc::type> &LCs,
184 const vector<t_pppSatObs*> &obsVector, const t_prn &refPrn,
185 bool pseudoObsIonoAvailable, bool preProcessing) {
186 LOG.setf(ios::fixed);
187 char sys = refPrn.system();
188
189 // Detect Cycle Slips
190 // ------------------
191 if (detectCycleSlips(LCs, obsVector, refPrn, preProcessing) != success) {
192 return failure;
193 }
194 if (preProcessing && _obsPool->refSatChangeRequired(sys)) {
195 return success;
196 }
197
198 ColumnVector xSav = _xFlt;
199 SymmetricMatrix QSav = _QFlt;
200 string epoTimeStr = string(_epoTime);
201 const vector<t_pppParam*> &params = _parlist.params();
202 unsigned nPar = _parlist.nPar();
203
204 unsigned usedLCs = LCs.size();
205 if (OPT->_pseudoObsIono && !pseudoObsIonoAvailable) {
206 usedLCs -= 1; // GIM not used
207 }
208 // max Obs
209 unsigned maxObs = obsVector.size() * usedLCs;
210
211 if (OPT->_pseudoObsIono && pseudoObsIonoAvailable) {
212 maxObs -= 1; // pseudo obs iono with respect to refSat
213 }
214
215 // Outlier Detection Loop
216 // ----------------------
217 for (unsigned iOutlier = 0; iOutlier < maxObs; iOutlier++) {
218
219 if (iOutlier > 0) {
220 _xFlt = xSav;
221 _QFlt = QSav;
222 }
223
224 // First-Design Matrix, Terms Observed-Computed, Weight Matrix
225 // -----------------------------------------------------------
226 Matrix AA(maxObs, nPar);
227 ColumnVector ll(maxObs);
228 DiagonalMatrix PP(maxObs);
229 PP = 0.0;
230
231 int iObs = -1;
232 vector<t_pppSatObs*> usedObs;
233 vector<t_lc::type> usedTypes;
234
235 // Real Observations
236 // =================
237 double nSat = 0;
238 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
239 t_pppSatObs *obs = obsVector[ii];
240 if (iOutlier == 0 && !preProcessing) {
241 obs->resetOutlier();
242 }
243 if (!obs->outlier()) {
244 nSat++;
245 for (unsigned jj = 0; jj < usedLCs; jj++) {
246 const t_lc::type tLC = LCs[jj];
247 if (tLC == t_lc::GIM) {
248 continue;
249 }
250 ++iObs;
251 usedObs.push_back(obs);
252 usedTypes.push_back(tLC);
253 for (unsigned iPar = 0; iPar < nPar; iPar++) {
254 const t_pppParam *par = params[iPar];
255 AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
256 }
257 ll[iObs] = obs->obsValue(tLC) - obs->cmpValue(tLC)
258 - DotProduct(_x0, AA.Row(iObs + 1));
259 PP[iObs] = 1.0 / (obs->sigma(tLC) * obs->sigma(tLC));
260 }
261 }
262 }
263
264 // Check number of observations
265 // ----------------------------
266 if (iObs == -1) {
267 LOG << " number of observations == -1\n";
268 return failure;
269 }
270
271 if ((!iOutlier) && (OPT->_obsModelType == OPT->DCMcodeBias ||
272 OPT->_obsModelType == OPT->DCMphaseBias) && (!preProcessing)) {
273 _datumTrafo->updateIndices(sys, iObs + 1);
274 _datumTrafo->prepareAA(AA.SubMatrix(1, iObs + 1, 1, _parlist.nPar()), 1);
275 }
276
277 // Pseudo Obs Iono
278 // ================
279 if (OPT->_pseudoObsIono && pseudoObsIonoAvailable) {
280 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
281 t_pppSatObs *obs = obsVector[ii];
282 if (!obs->outlier()) {
283 for (unsigned jj = 0; jj < usedLCs; jj++) {
284 const t_lc::type tLC = LCs[jj];
285 if (tLC == t_lc::GIM && !obs->isReference()) {
286 ++iObs;
287 } else {
288 continue;
289 }
290 usedObs.push_back(obs);
291 usedTypes.push_back(tLC);
292 for (unsigned iPar = 0; iPar < nPar; iPar++) {
293 const t_pppParam *par = params[iPar];
294 AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
295 }
296 ll[iObs] = obs->obsValue(tLC) - obs->cmpValue(tLC)
297 - DotProduct(_x0, AA.Row(iObs + 1));
298 PP[iObs] = 1.0 / (obs->sigma(tLC) * obs->sigma(tLC));
299 }
300 }
301 }
302 }
303
304 // Truncate matrices
305 // -----------------
306 AA = AA.Rows(1, iObs + 1);
307 ll = ll.Rows(1, iObs + 1);
308 PP = PP.SymSubMatrix(1, iObs + 1);
309
310 // Kalman update step
311 // ------------------
312 kalman(AA, ll, PP, _QFlt, _xFlt);
313
314 // Check Residuals
315 // ---------------
316 ColumnVector vv = AA * _xFlt - ll;
317 double maxOutlier = 0.0;
318 int maxOutlierIndex = -1;
319 t_lc::type maxOutlierLC = t_lc::dummy;
320 for (unsigned ii = 0; ii < usedObs.size(); ii++) {
321 const t_lc::type tLC = usedTypes[ii];
322 double res = fabs(vv[ii]);
323 if (res > usedObs[ii]->maxRes(tLC)) {
324 if (res > fabs(maxOutlier)) {
325 maxOutlier = vv[ii];
326 maxOutlierIndex = ii;
327 maxOutlierLC = tLC;
328 }
329 }
330 }
331
332 // Mark outlier or break outlier detection loop
333 // --------------------------------------------
334 if (maxOutlierIndex > -1) {
335 t_pppSatObs *obs = usedObs[maxOutlierIndex];
336 t_pppParam *par = 0;
337 for (unsigned iPar = 0; iPar < nPar; iPar++) {
338 t_pppParam *hlp = params[iPar];
339 if (hlp->type() == t_pppParam::amb && hlp->prn() == obs->prn()
340 && hlp->tLC() == usedTypes[maxOutlierIndex]) {
341 par = hlp;
342 }
343 }
344 if (preProcessing) {
345 // for refSats no ambiguity parameter exists
346 if ((obs->prn() == refPrn)
347 && (t_lc::toString(maxOutlierLC) == "l1" ||
348 t_lc::toString(maxOutlierLC) == "l2")) {
349 _obsPool->setRefSatChangeRequired(sys, true);
350 LOG << epoTimeStr << " Outlier ("
351 << ((preProcessing) ? "pre-processing) " : "fin-processing) ")
352 << t_lc::toString(maxOutlierLC) << ' ' << obs->prn().toString()
353 << ' ' << setw(8) << setprecision(4) << maxOutlier << endl;
354 break;
355 } else {
356 obs->setOutlier();
357 }
358 } else { // fin-processing
359 LOG << epoTimeStr << " Outlier " << t_lc::toString(maxOutlierLC) << ' '
360 << obs->prn().toString() << ' ' << setw(8) << setprecision(4)
361 << maxOutlier << endl;
362 if (par) {
363 if ( par->ambResetCandidate() ||
364 (OPT->_obsModelType == OPT->DCMcodeBias ||
365 OPT->_obsModelType == OPT->DCMphaseBias) ) {
366 resetAmb(par->prn(), obsVector, &QSav, &xSav);
367 }
368 else {
369 par->setAmbResetCandidate();
370 obs->setOutlier();
371 }
372 }
373 else {
374 obs->setOutlier();
375 }
376 }
377 }
378 // Print Residuals
379 // ---------------
380 else {
381 if (!preProcessing) {
382 for (unsigned jj = 0; jj < LCs.size(); jj++) {
383 for (unsigned ii = 0; ii < usedObs.size(); ii++) {
384 const t_lc::type tLC = usedTypes[ii];
385 t_pppSatObs *obs = usedObs[ii];
386 if (tLC == LCs[jj]) {
387 obs->setRes(tLC, vv[ii]);
388 LOG << epoTimeStr << " RES " << left << setw(3)
389 << t_lc::toString(tLC) << right << ' '
390 << obs->prn().toString();
391 LOG << setw(9) << setprecision(4) << vv[ii] << endl;
392 }
393 }
394 }
395 }
396 break;
397 }
398 }
399 return success;
400}
401
402// Cycle-Slip Detection
403////////////////////////////////////////////////////////////////////////////
404t_irc t_pppFilter::detectCycleSlips(const vector<t_lc::type> &LCs,
405 const vector<t_pppSatObs*> &obsVector, const t_prn &refPrn,
406 bool preProcessing) {
407 const double SLIP = 20.0;
408 char sys = refPrn.system();
409 string epoTimeStr = string(_epoTime);
410 const vector<t_pppParam*> &params = _parlist.params();
411 unsigned nPar = _parlist.nPar();
412
413 for (unsigned ii = 0; ii < LCs.size(); ii++) {
414 const t_lc::type &tLC = LCs[ii];
415 if (t_lc::includesPhase(tLC)) {
416 for (unsigned iObs = 0; iObs < obsVector.size(); iObs++) {
417 const t_pppSatObs *obs = obsVector[iObs];
418
419 // Check set Slips and Jump Counters
420 // ---------------------------------
421 bool slip = false;
422
423 // in pre-processing only the reference satellite has to be checked
424 if (preProcessing && obs->prn() != refPrn) {
425 continue;
426 }
427
428 if (obs->slip()) {
429 LOG << epoTimeStr << "cycle slip set (obs) " << obs->prn().toString()
430 << endl;
431 slip = true;
432 }
433
434 if (_slips[obs->prn()]._obsSlipCounter != -1
435 && _slips[obs->prn()]._obsSlipCounter != obs->slipCounter()) {
436 LOG << epoTimeStr << "cycle slip set (obsSlipCounter) "
437 << obs->prn().toString() << endl;
438 slip = true;
439 }
440 if (!preProcessing) {
441 _slips[obs->prn()]._obsSlipCounter = obs->slipCounter();
442 }
443 if (_slips[obs->prn()]._biasJumpCounter != -1
444 && _slips[obs->prn()]._biasJumpCounter != obs->biasJumpCounter()) {
445 LOG << epoTimeStr << "cycle slip set (biasJumpCounter) "
446 << obs->prn().toString() << endl;
447 slip = true;
448 }
449 if (!preProcessing) {
450 _slips[obs->prn()]._biasJumpCounter = obs->biasJumpCounter();
451 }
452 // Slip Set
453 // --------
454 if (slip) {
455 if (preProcessing) {
456 _obsPool->setRefSatChangeRequired(sys, true);
457 } else {
458 resetAmb(obs->prn(), obsVector);
459 }
460 }
461 // Check Pre-Fit Residuals
462 // -----------------------
463 else {
464 if (refPrn != t_prn()) {
465 return success;
466 }
467 ColumnVector AA(nPar);
468 for (unsigned iPar = 0; iPar < nPar; iPar++) {
469 const t_pppParam *par = params[iPar];
470 AA[iPar] = par->partial(_epoTime, obs, tLC, refPrn);
471 }
472 double ll = obs->obsValue(tLC) - obs->cmpValue(tLC)
473 - DotProduct(_x0, AA);
474 double vv = DotProduct(AA, _xFlt) - ll;
475 if (fabs(vv) > SLIP) {
476 LOG << epoTimeStr << " cycle slip detected " << t_lc::toString(tLC)
477 << ' ' << obs->prn().toString() << ' ' << setw(8)
478 << setprecision(4) << vv << endl;
479 if (preProcessing) {
480 _obsPool->setRefSatChangeRequired(sys, true);
481 } else {
482 resetAmb(obs->prn(), obsVector);
483 }
484 }
485 }
486 }
487 }
488 }
489 return success;
490}
491
492// Reset Ambiguity Parameter (cycle slip)
493////////////////////////////////////////////////////////////////////////////
494t_irc t_pppFilter::resetAmb(t_prn prn, const vector<t_pppSatObs*> &obsVector,
495 SymmetricMatrix *QSav, ColumnVector *xSav) {
496
497 t_irc irc = failure;
498 vector<t_pppParam*> &params = _parlist.params();
499 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
500 t_pppParam *par = params[iPar];
501 if (par->type() == t_pppParam::amb && par->prn() == prn) {
502 int ind = par->indexNew();
503 t_lc::type tLC = par->tLC();
504 LOG << string(_epoTime) << " RESET " << par->toString() << endl;
505 delete par;
506 par = new t_pppParam(t_pppParam::amb, prn, tLC, &obsVector);
507 par->setIndex(ind);
508 params[iPar] = par;
509 for (unsigned ii = 1; ii <= params.size(); ii++) {
510 _QFlt(ii, ind + 1) = 0.0;
511 if (QSav) {
512 (*QSav)(ii, ind + 1) = 0.0;
513 }
514 }
515 _QFlt(ind + 1, ind + 1) = par->sigma0() * par->sigma0();
516 if (QSav) {
517 (*QSav)(ind + 1, ind + 1) = _QFlt(ind + 1, ind + 1);
518 }
519 _xFlt[ind] = 0.0;
520 if (xSav) {
521 (*xSav)[ind] = _xFlt[ind];
522 }
523 _x0[ind] = par->x0();
524 irc = success;
525 }
526 }
527
528 return irc;
529}
530
531// Add infinite noise to iono
532////////////////////////////////////////////////////////////////////////////
533t_irc t_pppFilter::addNoiseToPar(t_pppParam::e_type parType, char sys) {
534 t_irc irc = failure;
535 vector<t_pppParam*> &params = _parlist.params();
536 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
537 t_pppParam *par = params[iPar];
538 if (par->type() == parType && par->prn().system() == sys) {
539 int ind = par->indexNew();
540 LOG << string(_epoTime) << " ADD NOISE TO " << par->toString() << 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
705 // set AA2
706 // =======
707 if (_parlist.set(_epoTime, allObs, refSatMap) != success) {
708 return failure;
709 }
710
711#ifdef BNC_DEBUG_PPP
712 _parlist.printParams(_epoTime);
713#endif
714
715 const QList<char> &usedSystems = _parlist.usedSystems();
716 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
717 char sys = usedSystems[iSys];
718 t_prn refPrn = refSatMap[sys]->prn();
719 vector<t_pppSatObs*> obsVector;
720 for (unsigned jj = 0; jj < allObs.size(); jj++) {
721 if (allObs[jj]->prn().system() == sys) {
722 allObs[jj]->resetOutlier();
723 obsVector.push_back(allObs[jj]);
724 }
725 }
726 if (iSys == 0) {
727 _datumTrafo->setFirstSystem(sys);
728 }
729 vector<t_lc::type> LCs = OPT->LCs(sys);
730 unsigned usedLCs = LCs.size();
731 if (OPT->_pseudoObsIono && !pseudoObsIono) {
732 usedLCs -= 1; // GIM not used
733 }
734 // max Obs
735 unsigned maxObs = obsVector.size() * usedLCs;
736
737 if (OPT->_pseudoObsIono && pseudoObsIono) {
738 maxObs -= 1; // pseudo obs iono with respect to refSat
739 }
740
741 const vector<t_pppParam*> &params = _parlist.params();
742 unsigned nPar = _parlist.nPar();
743
744 Matrix AA(maxObs, nPar);
745
746 // Real Observations
747 // -----------------
748 int iObs = -1;
749 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
750 t_pppSatObs *obs = obsVector[ii];
751 for (unsigned jj = 0; jj < usedLCs; jj++) {
752 const t_lc::type tLC = LCs[jj];
753 if (tLC == t_lc::GIM) {
754 continue;
755 }
756 ++iObs;
757 for (unsigned iPar = 0; iPar < nPar; iPar++) {
758 const t_pppParam *par = params[iPar];
759 AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
760 }
761 }
762 }
763
764 if (!iObs) {
765 continue;
766 }
767 _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;
768 if (_datumTrafo->prepareAA(AA.SubMatrix(1, iObs + 1, 1, nPar), 2)
769 != success) {
770 return failure;
771 }
772 }
773 _datumTrafo->updateNumObs();
774
775 // Datum Transformation
776 // ====================
777#ifdef BNC_DEBUG_PPP
778 //LOG << "AA1\n"; _datumTrafo->printMatrix(_datumTrafo->AA1(), _datumTrafo->numObs(), _datumTrafo->numPar());
779 //LOG << "AA2\n"; _datumTrafo->printMatrix(_datumTrafo->AA2(), _datumTrafo->numObs(), _datumTrafo->numPar());
780#endif
781 if (_datumTrafo->computeTrafoMatrix() != success) {
782 return failure;
783 }
784#ifdef BNC_DEBUG_PPP
785 //LOG << "D21" << endl; _datumTrafo->printMatrix(_datumTrafo->D21(), _datumTrafo->numObs(), _datumTrafo->numPar());
786#endif
787 ColumnVector xFltOld = _xFlt;
788 SymmetricMatrix QFltOld = _QFlt;
789
790 _QFlt << _datumTrafo->D21() * QFltOld * _datumTrafo->D21().t();
791 _xFlt = _datumTrafo->D21() * xFltOld;
792
793#ifdef BNC_DEBUG_PPP
794 LOG << "xFltOld:\n" << xFltOld << endl;
795 LOG << "xFlt :\n" << _xFlt << endl;
796#endif
797
798 // Reset Ambiguities after Datum Transformation
799 // ============================================
800 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
801 char sys = usedSystems[iSys];
802 t_prn refPrnOld = refSatMapLastEpoch[sys]->prn();
803 t_prn refPrnNew = refSatMap[sys]->prn();
804 if (refPrnNew != refPrnOld) {
805 resetAmb(refPrnOld, allObs);/*
806 if (resetAmb(refPrnOld, allObs) == success) {
807 if (OPT->_obsModelType == OPT->DCMcodeBias) {
808 addNoiseToPar(t_pppParam::ion, sys);
809 } else if (OPT->_obsModelType == OPT->DCMphaseBias) {
810 if (sys == 'G') {
811 addNoiseToPar(t_pppParam::pBiasG1, sys);
812 addNoiseToPar(t_pppParam::pBiasG2, sys);
813 } else if (sys == 'R') {
814 addNoiseToPar(t_pppParam::pBiasR1, sys);
815 addNoiseToPar(t_pppParam::pBiasR2, sys);
816 } else if (sys == 'E') {
817 addNoiseToPar(t_pppParam::pBiasE1, sys);
818 addNoiseToPar(t_pppParam::pBiasE2, sys);
819 } else if (sys == 'C') {
820 addNoiseToPar(t_pppParam::pBiasC1, sys);
821 addNoiseToPar(t_pppParam::pBiasC2, sys);
822 }
823 }
824 }*/
825 }
826 }
827
828 // switch AA2 to AA1
829 // =================
830 _datumTrafo->switchAA();
831
832 _obsPool->putEpoch(_epoTime, allObs, pseudoObsIono, refSatMap);
833
834 return success;
835}
836
837// Init datum transformation
838////////////////////////////////////////////////////////////////////////////
839void t_pppFilter::initDatumTransformation(
840 const std::vector<t_pppSatObs*> &allObs, bool pseudoObsIono) {
841 unsigned trafoObs = 0;
842 const QList<char> &usedSystems = _parlist.usedSystems();
843 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
844 char sys = usedSystems[iSys];
845 int satNum = 0;
846 for (unsigned jj = 0; jj < allObs.size(); jj++) {
847 if (allObs[jj]->prn().system() == sys) {
848 satNum++;
849 }
850 }
851 // all LCs
852 unsigned realUsedLCs = OPT->LCs(sys).size();
853 // exclude pseudo obs GIM
854 if (OPT->_pseudoObsIono && !pseudoObsIono) {
855 realUsedLCs -= 1;
856 }
857
858 trafoObs += satNum * realUsedLCs;
859
860 if (OPT->_pseudoObsIono && pseudoObsIono) {
861 trafoObs -= 1; // pseudo obs iono with respect to refSat
862 }
863 }
864 _datumTrafo->setNumObs(trafoObs);
865 _datumTrafo->setNumPar(_parlist.nPar());
866 _datumTrafo->initAA();
867}
868
869//
870//////////////////////////////////////////////////////////////////////////////
871bool t_pppFilter::resetRefSatellitesLastEpoch(
872 std::vector<t_pppSatObs*> &obsVector,
873 const QMap<char, t_pppRefSat*> &refSatMap,
874 const QMap<char, t_pppRefSat*> &refSatMapLastEpoch) {
875 bool resetRefSat;
876 // reference satellite definition per system
877 const QList<char> &usedSystems = _parlist.usedSystems();
878 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
879 resetRefSat = false;
880 char sys = usedSystems[iSys];
881 t_prn newPrn = refSatMap[sys]->prn();
882 t_prn oldPrn = refSatMapLastEpoch[sys]->prn();
883#ifdef BNC_DEBUG_PPP
884 if (oldPrn != newPrn) {
885 LOG << "oldRef: " << oldPrn.toString() << " => newRef " << newPrn.toString() << endl;
886 }
887#endif
888 vector<t_pppSatObs*>::iterator it = obsVector.begin();
889 while (it != obsVector.end()) {
890 t_pppSatObs *satObs = *it;
891 if (satObs->prn() == newPrn) {
892 resetRefSat = true;
893 satObs->setAsReference();
894 } else if (satObs->prn() == oldPrn) {
895 satObs->resetReference();
896 }
897 it++;
898 }
899 if (!resetRefSat) {
900 _obsPool->setRefSatChangeRequired(sys, true);
901 return resetRefSat;
902 }
903 }
904 return resetRefSat;
905}
Note: See TracBrowser for help on using the repository browser.