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

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

update 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() ||
67 !_lastEpoTimeOK.valid() ||
68 (maxSolGap > 0.0 && _epoTime - _lastEpoTimeOK > maxSolGap)) {
69 _firstEpoTime = _epoTime;
70 }
71
72 string epoTimeStr = string(_epoTime);
73
74 const QMap<char, t_pppRefSat*>& refSatMap = epoch->refSatMap();
75
76 //LOG << "processEpoch: printParams before set" << endl;
77 //_parlist.printParams(_epoTime);
78
79 //--
80 // Set Parameters
81 if (_parlist.set(_epoTime, allObs, refSatMap) != success) {
82 return failure;
83 }
84
85 if (OPT->_obsModelType == OPT->DCMcodeBias ||
86 OPT->_obsModelType == OPT->DCMphaseBias) {
87#ifdef BNC_DEBUG_PPP
88 _parlist.printParams(_epoTime);
89#endif
90 }
91 // Status Vector, Variance-Covariance Matrix
92 // -----------------------------------------
93 ColumnVector xFltOld = _xFlt;
94 SymmetricMatrix QFltOld = _QFlt;
95 setStateVectorAndVarCovMatrix(xFltOld, QFltOld);
96
97 // Pre-Process Satellite Systems separately
98 // ----------------------------------------
99 bool preProcessing = false;
100 if (OPT->_obsModelType == OPT->DCMcodeBias ||
101 OPT->_obsModelType == OPT->DCMphaseBias) {
102 preProcessing = true;
103 const QList<char>& usedSystems = _parlist.usedSystems();
104 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
105 char sys = usedSystems[iSys];
106 _refPrn.set(sys, 0);
107 if (OPT->_refSatRequired) {
108 _refPrn = refSatMap[sys]->prn();
109 }
110 vector<t_pppSatObs*> obsVector;
111 for (unsigned jj = 0; jj < allObs.size(); jj++) {
112 if (allObs[jj]->prn().system() == sys) {
113 obsVector.push_back(allObs[jj]);
114 }
115 }
116 if (iSys == 0) {
117 _datumTrafo->setFirstSystem(sys);
118 }
119 if (processSystem(OPT->LCs(sys), obsVector, _refPrn,
120 epoch->pseudoObsIono(), preProcessing) != success) {
121 LOG << "processSystem != success (pre-processing)" << endl;
122 _xFlt = xFltOld;
123 _QFlt = QFltOld;
124 _obsPool->deleteLastEpoch();
125 restoreState(2);
126 return failure;
127 }
128 }
129 // refSat change required?
130 // -----------------------
131 if (_obsPool->refSatChangeRequired()) {
132 _xFlt = xFltOld;
133 _QFlt = QFltOld;
134 return success;
135 }
136 else if (!_obsPool->refSatChangeRequired()) {
137 initDatumTransformation(allObs, epoch->pseudoObsIono());
138 }
139 }
140
141 // Process Satellite Systems separately
142 // ------------------------------------
143 preProcessing = false;
144 const QList<char>& usedSystems = _parlist. usedSystems();
145 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
146 char sys = usedSystems[iSys];
147 _refPrn.set(sys, 0);
148 if (OPT->_refSatRequired) {
149 _refPrn = refSatMap[sys]->prn();
150 }
151 unsigned int num = 0;
152 vector<t_pppSatObs*> obsVector;
153 for (unsigned jj = 0; jj < allObs.size(); jj++) {
154 if (allObs[jj]->prn().system() == sys) {
155 obsVector.push_back(allObs[jj]);
156 ++num;
157 }
158 }
159 if (iSys == 0 && OPT->_obsModelType == OPT->UncombPPP) {
160 _datumTrafo->setFirstSystem(sys);
161 }
162 LOG << epoTimeStr << " SATNUM " << sys << ' ' << right << setw(2) << num << endl;
163 if (processSystem(OPT->LCs(sys), obsVector, _refPrn,
164 epoch->pseudoObsIono(), preProcessing) != success) {
165 LOG << "processSystem != success (fin-processing)" << endl;
166 _xFlt = xFltOld;
167 _QFlt = QFltOld;
168 if (OPT->_obsModelType == OPT->DCMcodeBias ||
169 OPT->_obsModelType == OPT->DCMphaseBias) {
170 _obsPool->deleteLastEpoch();
171 restoreState(3);
172 }
173 return failure;
174 }
175 }
176
177 // close epoch processing
178 // ----------------------
179 cmpDOP(allObs, refSatMap);
180 _parlist.printResult(_epoTime, _QFlt, _xFlt);
181 _lastEpoTimeOK = _epoTime; // remember time of last successful epoch processing
182 return success;
183}
184
185// Process Selected LCs
186////////////////////////////////////////////////////////////////////////////
187t_irc t_pppFilter::processSystem(const vector<t_lc::type>& LCs,
188 const vector<t_pppSatObs*>& obsVector,
189 const t_prn& refPrn,
190 bool pseudoObsIonoAvailable,
191 bool preProcessing) {
192 LOG.setf(ios::fixed);
193 char sys = refPrn.system();
194
195 // Detect Cycle Slips
196 // ------------------
197 if (detectCycleSlips(LCs, obsVector, refPrn, preProcessing) != success) {
198 return failure;
199 }
200 if (preProcessing && _obsPool->refSatChangeRequired(sys)) {
201 return success;
202 }
203
204 ColumnVector xSav = _xFlt;
205 SymmetricMatrix QSav = _QFlt;
206 string epoTimeStr = string(_epoTime);
207 const vector<t_pppParam*>& params = _parlist.params();
208 unsigned nPar = _parlist.nPar();
209
210 unsigned usedLCs = LCs.size();
211 if (OPT->_pseudoObsIono && !pseudoObsIonoAvailable) {
212 usedLCs -= 1; // GIM not used
213 }
214 // max Obs
215 unsigned maxObs = obsVector.size() * usedLCs;
216
217 if (OPT->_pseudoObsIono && pseudoObsIonoAvailable) {
218 maxObs -= 1; // pseudo obs iono with respect to refSat
219 }
220
221 // Outlier Detection Loop
222 // ----------------------
223 for (unsigned iOutlier = 0; iOutlier < maxObs; iOutlier++) {
224
225 if (iOutlier > 0) {
226 _xFlt = xSav;
227 _QFlt = QSav;
228 }
229
230 // First-Design Matrix, Terms Observed-Computed, Weight Matrix
231 // -----------------------------------------------------------
232 Matrix AA(maxObs, nPar);
233 ColumnVector ll(maxObs);
234 DiagonalMatrix PP(maxObs); PP = 0.0;
235
236 int iObs = -1;
237 vector<t_pppSatObs*> usedObs;
238 vector<t_lc::type> usedTypes;
239
240 // Real Observations
241 // =================
242 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
243 t_pppSatObs* obs = obsVector[ii];
244 if (iOutlier == 0 && !preProcessing) {
245 obs->resetOutlier();
246 }
247 if (!obs->outlier()) {
248 for (unsigned jj = 0; jj < usedLCs; jj++) {
249 const t_lc::type tLC = LCs[jj];
250 if (tLC == t_lc::GIM) {continue;}
251 ++iObs;
252 usedObs.push_back(obs);
253 usedTypes.push_back(tLC);
254 for (unsigned iPar = 0; iPar < nPar; iPar++) {
255 const t_pppParam* par = params[iPar];
256 AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
257 }
258 ll[iObs] = obs->obsValue(tLC) - obs->cmpValue(tLC) - 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) {LOG << "iObs == -1\n";
267 return failure;
268 }
269 if (OPT->_obsModelType == OPT->DCMcodeBias ||
270 OPT->_obsModelType == OPT->DCMphaseBias ||
271 OPT->_pseudoObsIono) {
272 double numSat = floor(iObs/2.0);
273 if (numSat < 2.0) {
274 LOG << sys << ": numsat " << numSat << " but at least 2 satellites would be required" << endl;
275 return failure;
276 }
277 }
278
279 if ((!iOutlier) &&
280 (OPT->_obsModelType == OPT->DCMcodeBias ||
281 OPT->_obsModelType == OPT->DCMphaseBias) &&
282 (!preProcessing)) {
283 _datumTrafo->updateIndices(sys, iObs+1);
284 _datumTrafo->prepareAA(AA.SubMatrix(1, iObs+1 , 1, _parlist.nPar()), 1);
285 }
286
287 // Pseudo Obs Iono
288 // ================
289 if (OPT->_pseudoObsIono && pseudoObsIonoAvailable) {
290 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
291 t_pppSatObs* obs = obsVector[ii];
292 if (!obs->outlier()) {
293 for (unsigned jj = 0; jj < usedLCs; jj++) {
294 const t_lc::type tLC = LCs[jj];
295 if (tLC == t_lc::GIM && !obs->isReference()) {
296 ++iObs;
297 } else {continue;}
298 usedObs.push_back(obs);
299 usedTypes.push_back(tLC);
300 for (unsigned iPar = 0; iPar < nPar; iPar++) {
301 const t_pppParam* par = params[iPar];
302 AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
303 }
304 ll[iObs] = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA.Row(iObs+1));
305 PP[iObs] = 1.0 / (obs->sigma(tLC) * obs->sigma(tLC));
306 }
307 }
308 }
309 }
310
311 // Truncate matrices
312 // -----------------
313 AA = AA.Rows(1, iObs+1);
314 ll = ll.Rows(1, iObs+1);
315 PP = PP.SymSubMatrix(1, iObs+1);
316
317 // Kalman update step
318 // ------------------
319 kalman(AA, ll, PP, _QFlt, _xFlt);
320
321 // Check Residuals
322 // ---------------
323 ColumnVector vv = AA * _xFlt - ll;
324 double maxOutlier = 0.0;
325 int maxOutlierIndex = -1;
326 t_lc::type maxOutlierLC = t_lc::dummy;
327 for (unsigned ii = 0; ii < usedObs.size(); ii++) {
328 const t_lc::type tLC = usedTypes[ii];
329 double res = fabs(vv[ii]);
330 if (res > usedObs[ii]->maxRes(tLC)) {
331 if (res > fabs(maxOutlier)) {
332 maxOutlier = vv[ii];
333 maxOutlierIndex = ii;
334 maxOutlierLC = tLC;
335 }
336 }
337 }
338
339 // Mark outlier or break outlier detection loop
340 // --------------------------------------------
341 if (maxOutlierIndex > -1) {
342 t_pppSatObs* obs = usedObs[maxOutlierIndex];
343 t_pppParam* par = 0;
344 for (unsigned iPar = 0; iPar < nPar; iPar++) {
345 t_pppParam* hlp = params[iPar];
346 if (hlp->type() == t_pppParam::amb &&
347 hlp->prn() == obs->prn() &&
348 hlp->tLC() == usedTypes[maxOutlierIndex]) {
349 par = hlp;
350 }
351 }
352 if (preProcessing) {
353 // for refSats no ambiguity parameter exists
354 if ((obs->prn() == refPrn) &&
355 (t_lc::toString(maxOutlierLC) == "l1" ||
356 t_lc::toString(maxOutlierLC) == "l2") ) {
357 _obsPool->setRefSatChangeRequired(sys, true);
358 LOG << epoTimeStr << " Outlier ("
359 << ((preProcessing) ? "pre-processing) " : "fin-processing) ") << t_lc::toString(maxOutlierLC) << ' '
360 << obs->prn().toString() << ' ' << setw(8) << setprecision(4) << maxOutlier << endl;
361 break;
362 }
363 else {
364 if (par) {
365 par->setAmbResetCandidate();
366 }
367 }
368 }
369 else {// fin-processing
370 LOG << epoTimeStr << " Outlier " << t_lc::toString(maxOutlierLC) << ' '
371 << obs->prn().toString() << ' ' << setw(8) << setprecision(4) << maxOutlier << endl;
372 if (par) {
373 if (par->ambResetCandidate()) {
374 resetAmb(par->prn(), obsVector, &QSav, &xSav);
375 }
376 else {
377 par->setAmbResetCandidate();
378 obs->setOutlier();
379 }
380 }
381 else {
382 obs->setOutlier();
383 }
384 }
385 }
386 // Print Residuals
387 // ---------------
388 else {
389 if (!preProcessing) {
390 for (unsigned jj = 0; jj < LCs.size(); jj++) {
391 for (unsigned ii = 0; ii < usedObs.size(); ii++) {
392 const t_lc::type tLC = usedTypes[ii];
393 t_pppSatObs* obs = usedObs[ii];
394 if (tLC == LCs[jj]) {
395 obs->setRes(tLC, vv[ii]);
396 LOG << epoTimeStr << " RES "
397 << left << setw(3) << t_lc::toString(tLC) << right << ' ';
398 if (t_lc::toString(tLC) == "Tz0") {LOG << sys << " ";}
399 else {LOG << obs->prn().toString();}
400 LOG << setw(9) << setprecision(4) << vv[ii] << endl;
401 }
402 }
403 }
404 }
405 break;
406 }
407 }
408 return success;
409}
410
411// Cycle-Slip Detection
412////////////////////////////////////////////////////////////////////////////
413t_irc t_pppFilter::detectCycleSlips(const vector<t_lc::type>& LCs,
414 const vector<t_pppSatObs*>& obsVector,
415 const t_prn& refPrn,
416 bool preProcessing) {
417 const double SLIP = 20.0;
418 char sys = refPrn.system();
419 string epoTimeStr = string(_epoTime);
420 const vector<t_pppParam*>& params = _parlist.params();
421 unsigned nPar = _parlist.nPar();
422
423 for (unsigned ii = 0; ii < LCs.size(); ii++) {
424 const t_lc::type& tLC = LCs[ii];
425 if (t_lc::includesPhase(tLC)) {
426 for (unsigned iObs = 0; iObs < obsVector.size(); iObs++) {
427 const t_pppSatObs* obs = obsVector[iObs];
428
429 // Check set Slips and Jump Counters
430 // ---------------------------------
431 bool slip = false;
432
433 // in pre-processing only the reference satellite has to be checked
434 if (preProcessing && obs->prn() != refPrn) {
435 continue;
436 }
437
438 if (obs->slip()) {
439 LOG << epoTimeStr << "cycle slip set (obs) " << obs->prn().toString() << endl;
440 slip = true;
441 }
442
443 if (_slips[obs->prn()]._obsSlipCounter != -1 &&
444 _slips[obs->prn()]._obsSlipCounter != obs->slipCounter()) {
445 LOG << epoTimeStr << "cycle slip set (obsSlipCounter) " << obs->prn().toString() << endl;
446 slip = true;
447 }
448 if (!preProcessing) {
449 _slips[obs->prn()]._obsSlipCounter = obs->slipCounter();
450 }
451 if (_slips[obs->prn()]._biasJumpCounter != -1 &&
452 _slips[obs->prn()]._biasJumpCounter != obs->biasJumpCounter()) {
453 LOG << epoTimeStr << "cycle slip set (biasJumpCounter) " << obs->prn().toString() << endl;
454 slip = true;
455 }
456 if (!preProcessing) {
457 _slips[obs->prn()]._biasJumpCounter = obs->biasJumpCounter();
458 }
459 // Slip Set
460 // --------
461 if (slip) {
462 if (preProcessing) {
463 _obsPool->setRefSatChangeRequired(sys, true);
464 }
465 else {
466 resetAmb(obs->prn(), obsVector);
467 }
468 }
469 // Check Pre-Fit Residuals
470 // -----------------------
471 else {
472 if (refPrn != t_prn()) {return success;}
473 ColumnVector AA(nPar);
474 for (unsigned iPar = 0; iPar < nPar; iPar++) {
475 const t_pppParam* par = params[iPar];
476 AA[iPar] = par->partial(_epoTime, obs, tLC, refPrn);
477 }
478 double ll = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA);
479 double vv = DotProduct(AA, _xFlt) - ll;
480 if (fabs(vv) > SLIP) {
481 LOG << epoTimeStr << " cycle slip detected " << t_lc::toString(tLC) << ' '
482 << obs->prn().toString() << ' ' << setw(8) << setprecision(4) << vv << endl;
483 if (preProcessing) {
484 _obsPool->setRefSatChangeRequired(sys, true);
485 }
486 else {
487 resetAmb(obs->prn(), obsVector);
488 }
489 }
490 }
491 }
492 }
493 }
494 return success;
495}
496
497// Reset Ambiguity Parameter (cycle slip)
498////////////////////////////////////////////////////////////////////////////
499t_irc t_pppFilter::resetAmb(t_prn prn, const vector<t_pppSatObs*>& obsVector,
500 SymmetricMatrix* QSav, ColumnVector* xSav) {
501
502 t_irc irc = failure;
503 vector<t_pppParam*>& params = _parlist.params();
504 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
505 t_pppParam* par = params[iPar];
506 if (par->type() == t_pppParam::amb && par->prn() == prn) {
507 int ind = par->indexNew();
508 t_lc::type tLC = par->tLC();
509 LOG << string(_epoTime) << " RESET " << par->toString() << endl;
510 delete par; par = new t_pppParam(t_pppParam::amb, prn, tLC, &obsVector);
511 par->setIndex(ind);
512 params[iPar] = par;
513 for (unsigned ii = 1; ii <= params.size(); ii++) {
514 _QFlt(ii, ind+1) = 0.0;
515 if (QSav) {
516 (*QSav)(ii, ind+1) = 0.0;
517 }
518 }
519 _QFlt(ind+1,ind+1) = par->sigma0() * par->sigma0();
520 if (QSav) {
521 (*QSav)(ind+1,ind+1) = _QFlt(ind+1,ind+1);
522 }
523 _xFlt[ind] = 0.0;
524 if (xSav) {
525 (*xSav)[ind] = _xFlt[ind];
526 }
527 _x0[ind] = par->x0();
528 irc = success;
529 }
530 }
531
532 return irc;
533}
534
535// Add infinite noise to iono
536////////////////////////////////////////////////////////////////////////////
537t_irc t_pppFilter::addNoiseToIono(char sys) {
538
539 t_irc irc = failure;
540 vector<t_pppParam*>& params = _parlist.params();
541 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
542 t_pppParam* par = params[iPar];
543 if (par->type() == t_pppParam::ion && par->prn().system() == sys) {
544 int ind = par->indexNew();
545 LOG << string(_epoTime) << " ADD IONO_NOISE TO " << par->prn().toString() << endl;
546 par->setIndex(ind);
547 _QFlt(ind+1,ind+1) += par->sigma0() * par->sigma0();
548 irc = success;
549 }
550 }
551
552 return irc;
553}
554
555// Compute various DOP Values
556////////////////////////////////////////////////////////////////////////////
557void t_pppFilter::cmpDOP(const vector<t_pppSatObs*>& obsVector,
558 const QMap<char, t_pppRefSat*>& refSatMap) {
559
560 _dop.reset();
561
562 try {
563 const unsigned numPar = 4;
564 Matrix AA(obsVector.size(), numPar);
565 _numSat = 0;
566 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
567 t_pppSatObs* obs = obsVector[ii];
568 char sys = obs->prn().system();
569 t_prn refPrn = t_prn();
570 if (OPT->_refSatRequired) {
571 refPrn = refSatMap[sys]->prn();
572 }
573 if (obs->isValid() && !obs->outlier()) {
574 ++_numSat;
575 for (unsigned iPar = 0; iPar < numPar; iPar++) {
576 const t_pppParam* par = _parlist.params()[iPar];
577 AA[_numSat-1][iPar] = par->partial(_epoTime, obs, t_lc::c1, refPrn);
578 }
579 }
580 }
581 if (_numSat < 4) {
582 return;
583 }
584 AA = AA.Rows(1, _numSat);
585 SymmetricMatrix NN; NN << AA.t() * AA;
586 SymmetricMatrix QQ = NN.i();
587
588 _dop.H = sqrt(QQ(1,1) + QQ(2,2));
589 _dop.V = sqrt(QQ(3,3));
590 _dop.P = sqrt(QQ(1,1) + QQ(2,2) + QQ(3,3));
591 _dop.T = sqrt(QQ(4,4));
592 _dop.G = sqrt(QQ(1,1) + QQ(2,2) + QQ(3,3) + QQ(4,4));
593 }
594 catch (...) {
595 }
596}
597
598//
599////////////////////////////////////////////////////////////////////////////
600void t_pppFilter::predictCovCrdPart(const SymmetricMatrix& QFltOld) {
601
602 const vector<t_pppParam*>& params = _parlist.params();
603 if (params.size() < 3) {
604 return;
605 }
606
607 bool first = (params[0]->indexOld() < 0);
608
609 SymmetricMatrix Qneu(3); Qneu = 0.0;
610 for (unsigned ii = 0; ii < 3; ii++) {
611 const t_pppParam* par = params[ii];
612 if (first) {
613 Qneu[ii][ii] = par->sigma0() * par->sigma0();
614 }
615 else {
616 Qneu[ii][ii] = par->noise() * par->noise();
617 }
618 }
619
620 const t_pppStation* sta = PPP_CLIENT->staRover();
621 SymmetricMatrix Qxyz(3);
622 covariNEU_XYZ(Qneu, sta->ellApr().data(), Qxyz);
623
624 if (first) {
625 _QFlt.SymSubMatrix(1,3) = Qxyz;
626 }
627 else {
628 double dt = _epoTime - _firstEpoTime;
629 if (dt < OPT->_seedingTime) {
630 _QFlt.SymSubMatrix(1,3) = QFltOld.SymSubMatrix(1,3);
631 }
632 else {
633 _QFlt.SymSubMatrix(1,3) = QFltOld.SymSubMatrix(1,3) + Qxyz;
634 }
635 }
636}
637
638//
639////////////////////////////////////////////////////////////////////////////
640void t_pppFilter::setStateVectorAndVarCovMatrix(const ColumnVector& xFltOld,
641 const SymmetricMatrix& QFltOld) {
642
643 const vector<t_pppParam*>& params = _parlist.params();
644 unsigned nPar = params.size();
645
646 _QFlt.ReSize(nPar); _QFlt = 0.0;
647 _xFlt.ReSize(nPar); _xFlt = 0.0;
648 _x0.ReSize(nPar); _x0 = 0.0;
649
650 for (unsigned ii = 0; ii < nPar; ii++) {
651 t_pppParam* par1 = params[ii];
652 if (QFltOld.size() == 0) {
653 par1->resetIndex();
654 }
655 _x0[ii] = par1->x0();
656 int iOld = par1->indexOld();
657 if (iOld < 0) {
658 _QFlt[ii][ii] = par1->sigma0() * par1->sigma0(); // new parameter
659 }
660 else {
661 _QFlt[ii][ii] = QFltOld[iOld][iOld] + par1->noise() * par1->noise();
662 _xFlt[ii] = xFltOld[iOld];
663 for (unsigned jj = 0; jj < ii; jj++) {
664 t_pppParam* par2 = params[jj];
665 int jOld = par2->indexOld();
666 if (jOld >= 0) {
667 _QFlt[ii][jj] = QFltOld(iOld+1,jOld+1);
668 }
669 }
670 }
671 }
672 predictCovCrdPart(QFltOld);
673}
674
675// Compute datum transformation
676////////////////////////////////////////////////////////////////////////////
677t_irc t_pppFilter::datumTransformation(const QMap<char, t_pppRefSat*>& refSatMap) {
678
679 // get last epoch
680 t_pppObsPool::t_epoch* epoch = _obsPool->lastEpoch();
681 if (!epoch) {
682 LOG << "t_pppFilter::datumTransformation: !lastEpoch" << endl;
683 return failure;
684 }
685 _epoTime = epoch->epoTime();
686 LOG.setf(ios::fixed);
687 LOG << string(_epoTime) << " DATUM TRANSFORMATION " << endl;
688
689 vector<t_pppSatObs*>& allObs = epoch->obsVector();
690
691 const QMap<char, t_pppRefSat*>& refSatMapLastEpoch = epoch->refSatMap();
692
693 bool pseudoObsIono = epoch->pseudoObsIono();
694
695 // reset old and set new refSats in last epoch (ambiguities/GIM)
696 // =============================================================
697 if (resetRefSatellitesLastEpoch(allObs, refSatMap, refSatMapLastEpoch) != true) {
698 LOG << "datumTransformation: resetRefSatellitesLastEpoch != success" << endl;
699 return failure;
700 }
701
702 if (OPT->_obsModelType == OPT->UncombPPP) {
703 _obsPool->putEpoch(_epoTime, allObs, pseudoObsIono, refSatMap);
704 return success;
705 }
706 //LOG << "datumTransformation: printParams before set" << endl;
707 //_parlist.printParams(_epoTime);
708
709 // set AA2
710 // =======
711 if (_parlist.set(_epoTime, allObs, refSatMap) != success) {
712 return failure;
713 }
714
715#ifdef BNC_DEBUG_PPP
716 _parlist.printParams(_epoTime);
717#endif
718
719 const QList<char>& usedSystems = _parlist.usedSystems();
720 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
721 char sys = usedSystems[iSys];
722 t_prn refPrn = refSatMap[sys]->prn();
723 vector<t_pppSatObs*> obsVector;
724 for (unsigned jj = 0; jj < allObs.size(); jj++) {
725 if (allObs[jj]->prn().system() == sys) {
726 obsVector.push_back(allObs[jj]);
727 }
728 }
729 if (iSys == 0) {
730 _datumTrafo->setFirstSystem(sys);
731 }
732 vector<t_lc::type> LCs = OPT->LCs(sys);
733 unsigned usedLCs = LCs.size();
734 if (OPT->_pseudoObsIono && !pseudoObsIono) {
735 usedLCs -= 1; // GIM not used
736 }
737 // max Obs
738 unsigned maxObs = obsVector.size() * usedLCs;
739
740 if (OPT->_pseudoObsIono && pseudoObsIono) {
741 maxObs -= 1; // pseudo obs iono with respect to refSat
742 }
743
744 const vector<t_pppParam*>& params = _parlist.params();
745 unsigned nPar = _parlist.nPar();
746
747 Matrix AA(maxObs, nPar);
748
749 // Real Observations
750 // -----------------
751 int iObs = -1;
752 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
753 t_pppSatObs* obs = obsVector[ii];
754 for (unsigned jj = 0; jj < usedLCs; jj++) {
755 const t_lc::type tLC = LCs[jj];
756 if (tLC == t_lc::GIM) {continue;}
757 ++iObs;
758 for (unsigned iPar = 0; iPar < nPar; iPar++) {
759 const t_pppParam* par = params[iPar];
760 AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
761 }
762 }
763 }
764
765 if (!iObs) {
766 continue;
767 }
768 _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;
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#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 t_irc irc = resetAmb(refPrnOld, allObs);
806 if (OPT->_obsModelType == OPT->DCMcodeBias) {
807 if (irc == success) {
808 addNoiseToIono(sys);
809 }
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(const std::vector<t_pppSatObs*>& allObs,
826 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(std::vector<t_pppSatObs*>& obsVector,
858 const QMap<char, t_pppRefSat*>& refSatMap,
859 const QMap<char, t_pppRefSat*>& refSatMapLastEpoch) {
860 bool resetRefSat;
861 // reference satellite definition per system
862 const QList<char>& usedSystems = _parlist.usedSystems();
863 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
864 resetRefSat = false;
865 char sys = usedSystems[iSys];
866 t_prn newPrn = refSatMap[sys]->prn();
867 t_prn oldPrn = refSatMapLastEpoch[sys]->prn();
868#ifdef BNC_DEBUG_PPP
869 if (oldPrn != newPrn) {
870 LOG << "oldRef: " << oldPrn.toString() << " => newRef " << newPrn.toString() << endl;
871 }
872#endif
873 vector<t_pppSatObs*>::iterator it = obsVector.begin();
874 while (it != obsVector.end()) {
875 t_pppSatObs* satObs = *it;
876 if (satObs->prn() == newPrn) {
877 resetRefSat = true;
878 satObs->setAsReference();
879 } else if (satObs->prn() == oldPrn) {
880 satObs->resetReference();
881 }
882 it++;
883 }
884 if (!resetRefSat) {
885 _obsPool->setRefSatChangeRequired(sys, true);
886 return resetRefSat;
887 }
888 }
889 return resetRefSat;
890}
Note: See TracBrowser for help on using the repository browser.