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

Last change on this file since 10008 was 10008, checked in by stuerze, 13 months ago

minor changes
  • Property svn:keywords set to Author Date Id Rev URL;svn:eol-style=native
  • Property svn:mime-type set to text/plain
File size: 26.8 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 == " << iObs + 1 << "\n";
268 if (preProcessing) {
269 _obsPool->setRefSatChangeRequired(sys, true);
270 return success;
271 }
272 return failure;
273 }
274
275 if ((!iOutlier) &&
276 (OPT->_obsModelType == OPT->DCMcodeBias ||
277 OPT->_obsModelType == OPT->DCMphaseBias) && (!preProcessing)) {
278 _datumTrafo->updateIndices(sys, iObs + 1);
279 _datumTrafo->prepareAA(AA.SubMatrix(1, iObs + 1, 1, _parlist.nPar()), 1);
280 }
281
282 // Pseudo Obs Iono
283 // ================
284 if (OPT->_pseudoObsIono && pseudoObsIonoAvailable) {
285 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
286 t_pppSatObs *obs = obsVector[ii];
287 if (!obs->outlier()) {
288 for (unsigned jj = 0; jj < usedLCs; jj++) {
289 const t_lc::type tLC = LCs[jj];
290 if (tLC == t_lc::GIM && !obs->isReference()) {
291 ++iObs;
292 } else {
293 continue;
294 }
295 usedObs.push_back(obs);
296 usedTypes.push_back(tLC);
297 for (unsigned iPar = 0; iPar < nPar; iPar++) {
298 const t_pppParam *par = params[iPar];
299 AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
300 }
301 ll[iObs] = obs->obsValue(tLC) - obs->cmpValue(tLC)
302 - DotProduct(_x0, AA.Row(iObs + 1));
303 PP[iObs] = 1.0 / (obs->sigma(tLC) * obs->sigma(tLC));
304 }
305 }
306 }
307 }
308
309 // Truncate matrices
310 // -----------------
311 AA = AA.Rows(1, iObs + 1);
312 ll = ll.Rows(1, iObs + 1);
313 PP = PP.SymSubMatrix(1, iObs + 1);
314
315 // Kalman update step
316 // ------------------
317 kalman(AA, ll, PP, _QFlt, _xFlt);
318
319 // Check Residuals
320 // ---------------
321 ColumnVector vv = AA * _xFlt - ll;
322 double maxOutlier = 0.0;
323 int maxOutlierIndex = -1;
324 t_lc::type maxOutlierLC = t_lc::dummy;
325 for (unsigned ii = 0; ii < usedObs.size(); ii++) {
326 const t_lc::type tLC = usedTypes[ii];
327 double res = fabs(vv[ii]);
328 if (res > usedObs[ii]->maxRes(tLC)) {
329 if (res > fabs(maxOutlier)) {
330 maxOutlier = vv[ii];
331 maxOutlierIndex = ii;
332 maxOutlierLC = tLC;
333 }
334 }
335 }
336
337 // Mark outlier or break outlier detection loop
338 // --------------------------------------------
339 if (maxOutlierIndex > -1) {
340 t_pppSatObs *obs = usedObs[maxOutlierIndex];
341 t_pppParam *par = 0;
342 for (unsigned iPar = 0; iPar < nPar; iPar++) {
343 t_pppParam *hlp = params[iPar];
344 if (hlp->type() == t_pppParam::amb &&
345 hlp->prn() == obs->prn() &&
346 hlp->tLC() == usedTypes[maxOutlierIndex]) {
347 par = hlp;
348 }
349 }
350 if (preProcessing) {
351 if (obs->prn() == refPrn) {
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 {obs->setOutlier();}
359 } else { // fin-processing
360 LOG << epoTimeStr << " Outlier " << t_lc::toString(maxOutlierLC) << ' '
361 << obs->prn().toString() << ' ' << setw(8) << setprecision(4)
362 << maxOutlier << endl;
363 if (par) {
364 if (par->ambResetCandidate() ||
365 OPT->_obsModelType == OPT->DCMcodeBias ||
366 OPT->_obsModelType == OPT->DCMphaseBias) {
367 resetAmb(par->prn(), obsVector, maxOutlierLC, &QSav, &xSav);
368 }
369 else {
370 par->setAmbResetCandidate();
371 obs->setOutlier();
372 }
373 }
374 else {
375 obs->setOutlier();
376 }
377 }
378 }
379 // Print Residuals
380 // ---------------
381 else {
382 if (!preProcessing) {
383 for (unsigned jj = 0; jj < LCs.size(); jj++) {
384 for (unsigned ii = 0; ii < usedObs.size(); ii++) {
385 const t_lc::type tLC = usedTypes[ii];
386 t_pppSatObs *obs = usedObs[ii];
387 if (tLC == LCs[jj]) {
388 obs->setRes(tLC, vv[ii]);
389 LOG << epoTimeStr << " RES " << left << setw(3)
390 << t_lc::toString(tLC) << right << ' '
391 << obs->prn().toString();
392 LOG << setw(9) << setprecision(4) << vv[ii] << endl;
393 }
394 }
395 }
396 }
397 break;
398 }
399 }
400 return success;
401}
402
403// Cycle-Slip Detection
404////////////////////////////////////////////////////////////////////////////
405t_irc t_pppFilter::detectCycleSlips(const vector<t_lc::type> &LCs,
406 const vector<t_pppSatObs*> &obsVector, const t_prn &refPrn,
407 bool preProcessing) {
408 const double SLIP = 20.0;
409 char sys = refPrn.system();
410 string epoTimeStr = string(_epoTime);
411 const vector<t_pppParam*> &params = _parlist.params();
412 unsigned nPar = _parlist.nPar();
413
414 for (unsigned ii = 0; ii < LCs.size(); ii++) {
415 const t_lc::type &tLC = LCs[ii];
416 if (t_lc::includesPhase(tLC)) {
417 for (unsigned iObs = 0; iObs < obsVector.size(); iObs++) {
418 const t_pppSatObs *obs = obsVector[iObs];
419
420 // Check set Slips and Jump Counters
421 // ---------------------------------
422 bool slip = false;
423
424 // in pre-processing only the reference satellite has to be checked
425 if (preProcessing && obs->prn() != refPrn) {
426 continue;
427 }
428
429 if (obs->slip()) {
430 LOG << epoTimeStr << "cycle slip set (obs) " << obs->prn().toString()
431 << endl;
432 slip = true;
433 }
434
435 if (_slips[obs->prn()]._obsSlipCounter != -1
436 && _slips[obs->prn()]._obsSlipCounter != obs->slipCounter()) {
437 LOG << epoTimeStr << "cycle slip set (obsSlipCounter) "
438 << obs->prn().toString() << endl;
439 slip = true;
440 }
441 if (!preProcessing) {
442 _slips[obs->prn()]._obsSlipCounter = obs->slipCounter();
443 }
444 if (_slips[obs->prn()]._biasJumpCounter != -1
445 && _slips[obs->prn()]._biasJumpCounter != obs->biasJumpCounter()) {
446 LOG << epoTimeStr << "cycle slip set (biasJumpCounter) "
447 << obs->prn().toString() << endl;
448 slip = true;
449 }
450 if (!preProcessing) {
451 _slips[obs->prn()]._biasJumpCounter = obs->biasJumpCounter();
452 }
453 // Slip Set
454 // --------
455 if (slip) {
456 if (preProcessing) {
457 _obsPool->setRefSatChangeRequired(sys, true);
458 } else {
459 resetAmb(obs->prn(), obsVector, tLC);
460 }
461 }
462 // Check Pre-Fit Residuals
463 // -----------------------
464 else {
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, tLC);
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 t_lc::type lc, 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 if (tLC != lc) {continue;}
503 LOG << string(_epoTime) << " RESET " << par->toString() << endl;
504 delete par;
505 par = new t_pppParam(t_pppParam::amb, prn, tLC, &obsVector);
506 par->setIndex(ind);
507 params[iPar] = par;
508 for (unsigned ii = 1; ii <= params.size(); ii++) {
509 _QFlt(ii, ind + 1) = 0.0;
510 if (QSav) {
511 (*QSav)(ii, ind + 1) = 0.0;
512 }
513 }
514 _QFlt(ind + 1, ind + 1) = par->sigma0() * par->sigma0();
515 if (QSav) {
516 (*QSav)(ind + 1, ind + 1) = _QFlt(ind + 1, ind + 1);
517 }
518 _xFlt[ind] = 0.0;
519 if (xSav) {
520 (*xSav)[ind] = _xFlt[ind];
521 }
522 _x0[ind] = par->x0();
523 irc = success;
524 }
525 }
526
527 return irc;
528}
529
530// Add infinite noise to iono
531////////////////////////////////////////////////////////////////////////////
532t_irc t_pppFilter::addNoiseToPar(t_pppParam::e_type parType, char sys) {
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 NOISE TO " << par->toString() << endl;
540 par->setIndex(ind);
541 _QFlt(ind + 1, ind + 1) = par->sigma0() * par->sigma0();
542 irc = success;
543 }
544 }
545
546 return irc;
547}
548
549// Compute various DOP Values
550////////////////////////////////////////////////////////////////////////////
551void t_pppFilter::cmpDOP(const vector<t_pppSatObs*> &obsVector,
552 const QMap<char, t_pppRefSat*> &refSatMap) {
553
554 _dop.reset();
555
556 try {
557 const unsigned numPar = 4;
558 Matrix AA(obsVector.size(), numPar);
559 _numSat = 0;
560 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
561 t_pppSatObs *obs = obsVector[ii];
562 char sys = obs->prn().system();
563 t_prn refPrn = t_prn();
564 if (OPT->_refSatRequired) {
565 refPrn = refSatMap[sys]->prn();
566 }
567 if (obs->isValid() && !obs->outlier()) {
568 ++_numSat;
569 for (unsigned iPar = 0; iPar < numPar; iPar++) {
570 const t_pppParam *par = _parlist.params()[iPar];
571 AA[_numSat - 1][iPar] = par->partial(_epoTime, obs, t_lc::c1, refPrn);
572 }
573 }
574 }
575 if (_numSat < 4) {
576 return;
577 }
578 AA = AA.Rows(1, _numSat);
579 SymmetricMatrix NN;
580 NN << AA.t() * AA;
581 SymmetricMatrix QQ = NN.i();
582
583 _dop.H = sqrt(QQ(1, 1) + QQ(2, 2));
584 _dop.V = sqrt(QQ(3, 3));
585 _dop.P = sqrt(QQ(1, 1) + QQ(2, 2) + QQ(3, 3));
586 _dop.T = sqrt(QQ(4, 4));
587 _dop.G = sqrt(QQ(1, 1) + QQ(2, 2) + QQ(3, 3) + QQ(4, 4));
588 } catch (...) {
589 }
590}
591
592//
593////////////////////////////////////////////////////////////////////////////
594void t_pppFilter::predictCovCrdPart(const SymmetricMatrix &QFltOld) {
595
596 const vector<t_pppParam*> &params = _parlist.params();
597 if (params.size() < 3) {
598 return;
599 }
600
601 bool first = (params[0]->indexOld() < 0);
602
603 SymmetricMatrix Qneu(3);
604 Qneu = 0.0;
605 for (unsigned ii = 0; ii < 3; ii++) {
606 const t_pppParam *par = params[ii];
607 if (first) {
608 Qneu[ii][ii] = par->sigma0() * par->sigma0();
609 } else {
610 Qneu[ii][ii] = par->noise() * par->noise();
611 }
612 }
613
614 const t_pppStation *sta = PPP_CLIENT->staRover();
615 SymmetricMatrix Qxyz(3);
616 covariNEU_XYZ(Qneu, sta->ellApr().data(), Qxyz);
617
618 if (first) {
619 _QFlt.SymSubMatrix(1, 3) = Qxyz;
620 } else {
621 double dt = _epoTime - _firstEpoTime;
622 if (dt < OPT->_seedingTime) {
623 _QFlt.SymSubMatrix(1, 3) = QFltOld.SymSubMatrix(1, 3);
624 } else {
625 _QFlt.SymSubMatrix(1, 3) = QFltOld.SymSubMatrix(1, 3) + Qxyz;
626 }
627 }
628}
629
630//
631////////////////////////////////////////////////////////////////////////////
632void t_pppFilter::setStateVectorAndVarCovMatrix(const ColumnVector &xFltOld,
633 const SymmetricMatrix &QFltOld) {
634
635 const vector<t_pppParam*> &params = _parlist.params();
636 unsigned nPar = params.size();
637
638 _QFlt.ReSize(nPar);
639 _QFlt = 0.0;
640 _xFlt.ReSize(nPar);
641 _xFlt = 0.0;
642 _x0.ReSize(nPar);
643 _x0 = 0.0;
644
645 for (unsigned ii = 0; ii < nPar; ii++) {
646 t_pppParam *par1 = params[ii];
647 if (QFltOld.size() == 0) {
648 par1->resetIndex();
649 }
650 _x0[ii] = par1->x0();
651 int iOld = par1->indexOld();
652 if (iOld < 0) {
653 _QFlt[ii][ii] = par1->sigma0() * par1->sigma0(); // new parameter
654 } else {
655 _QFlt[ii][ii] = QFltOld[iOld][iOld] + par1->noise() * par1->noise();
656 _xFlt[ii] = xFltOld[iOld];
657 for (unsigned jj = 0; jj < ii; jj++) {
658 t_pppParam *par2 = params[jj];
659 int jOld = par2->indexOld();
660 if (jOld >= 0) {
661 _QFlt[ii][jj] = QFltOld(iOld + 1, jOld + 1);
662 }
663 }
664 }
665 }
666 predictCovCrdPart(QFltOld);
667}
668
669// Compute datum transformation
670////////////////////////////////////////////////////////////////////////////
671t_irc t_pppFilter::datumTransformation(
672 const QMap<char, t_pppRefSat*> &refSatMap) {
673
674 // get last epoch
675 t_pppObsPool::t_epoch *epoch = _obsPool->lastEpoch();
676 if (!epoch) {
677 LOG << "t_pppFilter::datumTransformation: !lastEpoch" << endl;
678 return failure;
679 }
680 _epoTime = epoch->epoTime();
681 LOG.setf(ios::fixed);
682 LOG << string(_epoTime) << " DATUM TRANSFORMATION " << endl;
683
684 vector<t_pppSatObs*> &allObs = epoch->obsVector();
685
686 const QMap<char, t_pppRefSat*> &refSatMapLastEpoch = epoch->refSatMap();
687
688 bool pseudoObsIono = epoch->pseudoObsIono();
689
690 // reset old and set new refSats in last epoch (ambiguities/GIM)
691 // =============================================================
692 if (resetRefSatellitesLastEpoch(allObs, refSatMap, refSatMapLastEpoch) != true) {
693 LOG << "datumTransformation: resetRefSatellitesLastEpoch != success" << endl;
694 return failure;
695 }
696
697 if (OPT->_obsModelType == OPT->UncombPPP) {
698 _obsPool->putEpoch(_epoTime, allObs, pseudoObsIono, refSatMap);
699 return success;
700 }
701
702 // set AA2
703 // =======
704 if (_parlist.set(_epoTime, allObs, refSatMap) != success) {
705 return failure;
706 }
707
708#ifdef BNC_DEBUG_PPP
709 _parlist.printParams(_epoTime);
710#endif
711
712 const QList<char> &usedSystems = _parlist.usedSystems();
713 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
714 char sys = usedSystems[iSys];
715 t_prn refPrn = refSatMap[sys]->prn();
716 vector<t_pppSatObs*> obsVector;
717 for (unsigned jj = 0; jj < allObs.size(); jj++) {
718 if (allObs[jj]->prn().system() == sys) {
719 allObs[jj]->resetOutlier();
720 obsVector.push_back(allObs[jj]);
721 }
722 }
723 if (iSys == 0) {
724 _datumTrafo->setFirstSystem(sys);
725 }
726 vector<t_lc::type> LCs = OPT->LCs(sys);
727 unsigned usedLCs = LCs.size();
728 if (OPT->_pseudoObsIono && !pseudoObsIono) {
729 usedLCs -= 1; // GIM not used
730 }
731 // max Obs
732 unsigned maxObs = obsVector.size() * usedLCs;
733
734 if (OPT->_pseudoObsIono && pseudoObsIono) {
735 maxObs -= 1; // pseudo obs iono with respect to refSat
736 }
737
738 const vector<t_pppParam*> &params = _parlist.params();
739 unsigned nPar = _parlist.nPar();
740
741 Matrix AA(maxObs, nPar);
742
743 // Real Observations
744 // -----------------
745 int iObs = -1;
746 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
747 t_pppSatObs *obs = obsVector[ii];
748 for (unsigned jj = 0; jj < usedLCs; jj++) {
749 const t_lc::type tLC = LCs[jj];
750 if (tLC == t_lc::GIM) {
751 continue;
752 }
753 ++iObs;
754 for (unsigned iPar = 0; iPar < nPar; iPar++) {
755 const t_pppParam *par = params[iPar];
756 AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
757 }
758 }
759 }
760
761 if (!(iObs+1)) {
762 continue;
763 }
764 _datumTrafo->updateIndices(sys, iObs + 1);
765#ifdef BNC_DEBUG_PPP
766 LOG << "AA Ncols/Nrows: " << AA.Ncols() << "/" << AA.Nrows() << " nPar: " << nPar << endl;
767 LOG << "AA.SubMatrix(1 .. " << iObs+1 << " , 1 .. " << nPar << ")" << endl;
768#endif
769 if (_datumTrafo->prepareAA(AA.SubMatrix(1, iObs + 1, 1, nPar), 2) != success) {
770 return failure;
771 }
772 }
773 _datumTrafo->updateNumObs();
774
775 // Datum Transformation
776 // ====================
777 if (_datumTrafo->computeTrafoMatrix() != success) {
778 return failure;
779 }
780
781 ColumnVector xFltOld = _xFlt;
782 SymmetricMatrix QFltOld = _QFlt;
783
784 _QFlt << _datumTrafo->D21() * QFltOld * _datumTrafo->D21().t();
785 _xFlt = _datumTrafo->D21() * xFltOld;
786
787#ifdef BNC_DEBUG_PPP
788 LOG << "xFltOld:\n" << xFltOld << endl;
789 LOG << "xFlt :\n" << _xFlt << endl;
790#endif
791
792 // Reset Ambiguities after Datum Transformation
793 // ============================================
794 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
795 char sys = usedSystems[iSys];
796 vector<t_lc::type> LCs = OPT->LCs(sys);
797 unsigned usedLCs = LCs.size();
798 if (OPT->_pseudoObsIono && !pseudoObsIono) {
799 usedLCs -= 1; // GIM not used
800 }
801 t_prn refPrnOld = refSatMapLastEpoch[sys]->prn();
802 t_prn refPrnNew = refSatMap[sys]->prn();
803 if (refPrnNew != refPrnOld) {
804 for (unsigned jj = 0; jj < usedLCs; jj++) {
805 const t_lc::type tLC = LCs[jj];
806 if (tLC == t_lc::GIM) {
807 continue;
808 }
809 resetAmb(refPrnOld, allObs, tLC);
810 }
811 }
812 }
813
814 // switch AA2 to AA1
815 // =================
816 _datumTrafo->switchAA();
817
818 _obsPool->putEpoch(_epoTime, allObs, pseudoObsIono, refSatMap);
819
820 return success;
821}
822
823// Init datum transformation
824////////////////////////////////////////////////////////////////////////////
825void t_pppFilter::initDatumTransformation(
826 const std::vector<t_pppSatObs*> &allObs, bool pseudoObsIono) {
827 unsigned trafoObs = 0;
828 const QList<char> &usedSystems = _parlist.usedSystems();
829 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
830 char sys = usedSystems[iSys];
831 int satNum = 0;
832 for (unsigned jj = 0; jj < allObs.size(); jj++) {
833 if (allObs[jj]->prn().system() == sys) {
834 satNum++;
835 }
836 }
837 // all LCs
838 unsigned realUsedLCs = OPT->LCs(sys).size();
839 // exclude pseudo obs GIM
840 if (OPT->_pseudoObsIono && !pseudoObsIono) {
841 realUsedLCs -= 1;
842 }
843
844 trafoObs += satNum * realUsedLCs;
845
846 if (OPT->_pseudoObsIono && pseudoObsIono) {
847 trafoObs -= 1; // pseudo obs iono with respect to refSat
848 }
849 }
850 _datumTrafo->setNumObs(trafoObs);
851 _datumTrafo->setNumPar(_parlist.nPar());
852 _datumTrafo->initAA();
853}
854
855//
856//////////////////////////////////////////////////////////////////////////////
857bool t_pppFilter::resetRefSatellitesLastEpoch(
858 std::vector<t_pppSatObs*> &obsVector,
859 const QMap<char, t_pppRefSat*> &refSatMap,
860 const QMap<char, t_pppRefSat*> &refSatMapLastEpoch) {
861 bool resetRefSat;
862 // reference satellite definition per system
863 const QList<char> &usedSystems = _parlist.usedSystems();
864 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
865 resetRefSat = false;
866 char sys = usedSystems[iSys];
867 t_prn newPrn = refSatMap[sys]->prn();
868 t_prn oldPrn = refSatMapLastEpoch[sys]->prn();
869#ifdef BNC_DEBUG_PPP
870 if (oldPrn != newPrn) {
871 LOG << "oldRef: " << oldPrn.toString() << " => newRef " << newPrn.toString() << endl;
872 }
873#endif
874 vector<t_pppSatObs*>::iterator it = obsVector.begin();
875 while (it != obsVector.end()) {
876 t_pppSatObs *satObs = *it;
877 if (satObs->prn() == newPrn) {
878 resetRefSat = true;
879 satObs->setAsReference();
880 } else if (satObs->prn() == oldPrn) {
881 satObs->resetReference();
882 }
883 it++;
884 }
885 if (!resetRefSat) {
886 _obsPool->setRefSatChangeRequired(sys, true);
887 return resetRefSat;
888 }
889 }
890 return resetRefSat;
891}
Note: See TracBrowser for help on using the repository browser.