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

Last change on this file since 9703 was 9703, checked in by stuerze, 2 years 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: 27.9 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 // better results
366 if (par) {
367 if (par->ambResetCandidate()) {
368 resetAmb(par->prn(), obsVector, &QSav, &xSav);
369 }
370 else {
371 par->setAmbResetCandidate();
372 obs->setOutlier();
373 }
374 }/* as this?
375 if (par) {
376 resetAmb(par->prn(), obsVector, &QSav, &xSav);
377 } */
378 else {
379 obs->setOutlier();
380 }
381 }
382 }
383 // Print Residuals
384 // ---------------
385 else {
386 if (!preProcessing) {
387 for (unsigned jj = 0; jj < LCs.size(); jj++) {
388 for (unsigned ii = 0; ii < usedObs.size(); ii++) {
389 const t_lc::type tLC = usedTypes[ii];
390 t_pppSatObs *obs = usedObs[ii];
391 if (tLC == LCs[jj]) {
392 obs->setRes(tLC, vv[ii]);
393 LOG << epoTimeStr << " RES " << left << setw(3)
394 << t_lc::toString(tLC) << right << ' '
395 << obs->prn().toString();
396 LOG << setw(9) << setprecision(4) << vv[ii] << endl;
397 }
398 }
399 }
400 }
401 break;
402 }
403 }
404 return success;
405}
406
407// Cycle-Slip Detection
408////////////////////////////////////////////////////////////////////////////
409t_irc t_pppFilter::detectCycleSlips(const vector<t_lc::type> &LCs,
410 const vector<t_pppSatObs*> &obsVector, const t_prn &refPrn,
411 bool preProcessing) {
412 const double SLIP = 20.0;
413 char sys = refPrn.system();
414 string epoTimeStr = string(_epoTime);
415 const vector<t_pppParam*> &params = _parlist.params();
416 unsigned nPar = _parlist.nPar();
417
418 for (unsigned ii = 0; ii < LCs.size(); ii++) {
419 const t_lc::type &tLC = LCs[ii];
420 if (t_lc::includesPhase(tLC)) {
421 for (unsigned iObs = 0; iObs < obsVector.size(); iObs++) {
422 const t_pppSatObs *obs = obsVector[iObs];
423
424 // Check set Slips and Jump Counters
425 // ---------------------------------
426 bool slip = false;
427
428 // in pre-processing only the reference satellite has to be checked
429 if (preProcessing && obs->prn() != refPrn) {
430 continue;
431 }
432
433 if (obs->slip()) {
434 LOG << epoTimeStr << "cycle slip set (obs) " << obs->prn().toString()
435 << endl;
436 slip = true;
437 }
438
439 if (_slips[obs->prn()]._obsSlipCounter != -1
440 && _slips[obs->prn()]._obsSlipCounter != obs->slipCounter()) {
441 LOG << epoTimeStr << "cycle slip set (obsSlipCounter) "
442 << obs->prn().toString() << endl;
443 slip = true;
444 }
445 if (!preProcessing) {
446 _slips[obs->prn()]._obsSlipCounter = obs->slipCounter();
447 }
448 if (_slips[obs->prn()]._biasJumpCounter != -1
449 && _slips[obs->prn()]._biasJumpCounter != obs->biasJumpCounter()) {
450 LOG << epoTimeStr << "cycle slip set (biasJumpCounter) "
451 << obs->prn().toString() << endl;
452 slip = true;
453 }
454 if (!preProcessing) {
455 _slips[obs->prn()]._biasJumpCounter = obs->biasJumpCounter();
456 }
457 // Slip Set
458 // --------
459 if (slip) {
460 if (preProcessing) {
461 _obsPool->setRefSatChangeRequired(sys, true);
462 } else {
463 resetAmb(obs->prn(), obsVector);
464 }
465 }
466 // Check Pre-Fit Residuals
467 // -----------------------
468 else {
469 if (refPrn != t_prn()) {
470 return success;
471 }
472 ColumnVector AA(nPar);
473 for (unsigned iPar = 0; iPar < nPar; iPar++) {
474 const t_pppParam *par = params[iPar];
475 AA[iPar] = par->partial(_epoTime, obs, tLC, refPrn);
476 }
477 double ll = obs->obsValue(tLC) - obs->cmpValue(tLC)
478 - 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)
483 << setprecision(4) << vv << endl;
484 if (preProcessing) {
485 _obsPool->setRefSatChangeRequired(sys, true);
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;
511 par = new t_pppParam(t_pppParam::amb, prn, tLC, &obsVector);
512 par->setIndex(ind);
513 params[iPar] = par;
514 for (unsigned ii = 1; ii <= params.size(); ii++) {
515 _QFlt(ii, ind + 1) = 0.0;
516 if (QSav) {
517 (*QSav)(ii, ind + 1) = 0.0;
518 }
519 }
520 _QFlt(ind + 1, ind + 1) = par->sigma0() * par->sigma0();
521 if (QSav) {
522 (*QSav)(ind + 1, ind + 1) = _QFlt(ind + 1, ind + 1);
523 }
524 _xFlt[ind] = 0.0;
525 if (xSav) {
526 (*xSav)[ind] = _xFlt[ind];
527 }
528 _x0[ind] = par->x0();
529 irc = success;
530 }
531 }
532
533 return irc;
534}
535
536// Add infinite noise to iono
537////////////////////////////////////////////////////////////////////////////
538t_irc t_pppFilter::addNoiseToPar(t_pppParam::e_type parType, char sys) {
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() == parType && par->prn().system() == sys) {
544 int ind = par->indexNew();
545 LOG << string(_epoTime) << " ADD NOISE TO " << par->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;
586 NN << AA.t() * AA;
587 SymmetricMatrix QQ = NN.i();
588
589 _dop.H = sqrt(QQ(1, 1) + QQ(2, 2));
590 _dop.V = sqrt(QQ(3, 3));
591 _dop.P = sqrt(QQ(1, 1) + QQ(2, 2) + QQ(3, 3));
592 _dop.T = sqrt(QQ(4, 4));
593 _dop.G = sqrt(QQ(1, 1) + QQ(2, 2) + QQ(3, 3) + QQ(4, 4));
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);
610 Qneu = 0.0;
611 for (unsigned ii = 0; ii < 3; ii++) {
612 const t_pppParam *par = params[ii];
613 if (first) {
614 Qneu[ii][ii] = par->sigma0() * par->sigma0();
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 } else {
627 double dt = _epoTime - _firstEpoTime;
628 if (dt < OPT->_seedingTime) {
629 _QFlt.SymSubMatrix(1, 3) = QFltOld.SymSubMatrix(1, 3);
630 } else {
631 _QFlt.SymSubMatrix(1, 3) = QFltOld.SymSubMatrix(1, 3) + Qxyz;
632 }
633 }
634}
635
636//
637////////////////////////////////////////////////////////////////////////////
638void t_pppFilter::setStateVectorAndVarCovMatrix(const ColumnVector &xFltOld,
639 const SymmetricMatrix &QFltOld) {
640
641 const vector<t_pppParam*> &params = _parlist.params();
642 unsigned nPar = params.size();
643
644 _QFlt.ReSize(nPar);
645 _QFlt = 0.0;
646 _xFlt.ReSize(nPar);
647 _xFlt = 0.0;
648 _x0.ReSize(nPar);
649 _x0 = 0.0;
650
651 for (unsigned ii = 0; ii < nPar; ii++) {
652 t_pppParam *par1 = params[ii];
653 if (QFltOld.size() == 0) {
654 par1->resetIndex();
655 }
656 _x0[ii] = par1->x0();
657 int iOld = par1->indexOld();
658 if (iOld < 0) {
659 _QFlt[ii][ii] = par1->sigma0() * par1->sigma0(); // new parameter
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(
678 const QMap<char, t_pppRefSat*> &refSatMap) {
679
680 // get last epoch
681 t_pppObsPool::t_epoch *epoch = _obsPool->lastEpoch();
682 if (!epoch) {
683 LOG << "t_pppFilter::datumTransformation: !lastEpoch" << endl;
684 return failure;
685 }
686 _epoTime = epoch->epoTime();
687 LOG.setf(ios::fixed);
688 LOG << string(_epoTime) << " DATUM TRANSFORMATION " << endl;
689
690 vector<t_pppSatObs*> &allObs = epoch->obsVector();
691
692 const QMap<char, t_pppRefSat*> &refSatMapLastEpoch = epoch->refSatMap();
693
694 bool pseudoObsIono = epoch->pseudoObsIono();
695
696 // reset old and set new refSats in last epoch (ambiguities/GIM)
697 // =============================================================
698 if (resetRefSatellitesLastEpoch(allObs, refSatMap, refSatMapLastEpoch)
699 != true) {
700 LOG << "datumTransformation: resetRefSatellitesLastEpoch != success"
701 << endl;
702 return failure;
703 }
704
705 if (OPT->_obsModelType == OPT->UncombPPP) {
706 _obsPool->putEpoch(_epoTime, allObs, pseudoObsIono, refSatMap);
707 return success;
708 }
709 //LOG << "datumTransformation: printParams before set" << endl;
710 //_parlist.printParams(_epoTime);
711
712 // set AA2
713 // =======
714 if (_parlist.set(_epoTime, allObs, refSatMap) != success) {
715 return failure;
716 }
717
718#ifdef BNC_DEBUG_PPP
719 _parlist.printParams(_epoTime);
720#endif
721
722 const QList<char> &usedSystems = _parlist.usedSystems();
723 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
724 char sys = usedSystems[iSys];
725 t_prn refPrn = refSatMap[sys]->prn();
726 vector<t_pppSatObs*> obsVector;
727 for (unsigned jj = 0; jj < allObs.size(); jj++) {
728 if (allObs[jj]->prn().system() == sys) {
729 allObs[jj]->resetOutlier();
730 obsVector.push_back(allObs[jj]);
731 }
732 }
733 if (iSys == 0) {
734 _datumTrafo->setFirstSystem(sys);
735 }
736 vector<t_lc::type> LCs = OPT->LCs(sys);
737 unsigned usedLCs = LCs.size();
738 if (OPT->_pseudoObsIono && !pseudoObsIono) {
739 usedLCs -= 1; // GIM not used
740 }
741 // max Obs
742 unsigned maxObs = obsVector.size() * usedLCs;
743
744 if (OPT->_pseudoObsIono && pseudoObsIono) {
745 maxObs -= 1; // pseudo obs iono with respect to refSat
746 }
747
748 const vector<t_pppParam*> &params = _parlist.params();
749 unsigned nPar = _parlist.nPar();
750
751 Matrix AA(maxObs, nPar);
752
753 // Real Observations
754 // -----------------
755 int iObs = -1;
756 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
757 t_pppSatObs *obs = obsVector[ii];
758 for (unsigned jj = 0; jj < usedLCs; jj++) {
759 const t_lc::type tLC = LCs[jj];
760 if (tLC == t_lc::GIM) {
761 continue;
762 }
763 ++iObs;
764 for (unsigned iPar = 0; iPar < nPar; iPar++) {
765 const t_pppParam *par = params[iPar];
766 AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
767 }
768 }
769 }
770
771 if (!iObs) {
772 continue;
773 }
774 _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;
775 if (_datumTrafo->prepareAA(AA.SubMatrix(1, iObs + 1, 1, nPar), 2)
776 != success) {
777 return failure;
778 }
779 }
780 _datumTrafo->updateNumObs();
781
782 // Datum Transformation
783 // ====================
784#ifdef BNC_DEBUG_PPP
785 //LOG << "AA1\n"; _datumTrafo->printMatrix(_datumTrafo->AA1(), _datumTrafo->numObs(), _datumTrafo->numPar());
786 //LOG << "AA2\n"; _datumTrafo->printMatrix(_datumTrafo->AA2(), _datumTrafo->numObs(), _datumTrafo->numPar());
787#endif
788 if (_datumTrafo->computeTrafoMatrix() != success) {
789 return failure;
790 }
791#ifdef BNC_DEBUG_PPP
792 //LOG << "D21" << endl; _datumTrafo->printMatrix(_datumTrafo->D21(), _datumTrafo->numObs(), _datumTrafo->numPar());
793#endif
794 ColumnVector xFltOld = _xFlt;
795 SymmetricMatrix QFltOld = _QFlt;
796
797 _QFlt << _datumTrafo->D21() * QFltOld * _datumTrafo->D21().t();
798 _xFlt = _datumTrafo->D21() * xFltOld;
799
800#ifdef BNC_DEBUG_PPP
801 LOG << "xFltOld:\n" << xFltOld << endl;
802 LOG << "xFlt :\n" << _xFlt << endl;
803#endif
804
805 // Reset Ambiguities after Datum Transformation
806 // ============================================
807 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
808 char sys = usedSystems[iSys];
809 t_prn refPrnOld = refSatMapLastEpoch[sys]->prn();
810 t_prn refPrnNew = refSatMap[sys]->prn();
811 if (refPrnNew != refPrnOld) {
812 resetAmb(refPrnOld, allObs);/*
813 if (resetAmb(refPrnOld, allObs) == success) {
814 if (OPT->_obsModelType == OPT->DCMcodeBias) {
815 addNoiseToPar(t_pppParam::ion, sys);
816 } else if (OPT->_obsModelType == OPT->DCMphaseBias) {
817 if (sys == 'G') {
818 addNoiseToPar(t_pppParam::pBiasG1, sys);
819 addNoiseToPar(t_pppParam::pBiasG2, sys);
820 } else if (sys == 'R') {
821 addNoiseToPar(t_pppParam::pBiasR1, sys);
822 addNoiseToPar(t_pppParam::pBiasR2, sys);
823 } else if (sys == 'E') {
824 addNoiseToPar(t_pppParam::pBiasE1, sys);
825 addNoiseToPar(t_pppParam::pBiasE2, sys);
826 } else if (sys == 'C') {
827 addNoiseToPar(t_pppParam::pBiasC1, sys);
828 addNoiseToPar(t_pppParam::pBiasC2, sys);
829 }
830 }
831 }*/
832 }
833 }
834
835 // switch AA2 to AA1
836 // =================
837 _datumTrafo->switchAA();
838
839 _obsPool->putEpoch(_epoTime, allObs, pseudoObsIono, refSatMap);
840
841 return success;
842}
843
844// Init datum transformation
845////////////////////////////////////////////////////////////////////////////
846void t_pppFilter::initDatumTransformation(
847 const std::vector<t_pppSatObs*> &allObs, bool pseudoObsIono) {
848 unsigned trafoObs = 0;
849 const QList<char> &usedSystems = _parlist.usedSystems();
850 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
851 char sys = usedSystems[iSys];
852 int satNum = 0;
853 for (unsigned jj = 0; jj < allObs.size(); jj++) {
854 if (allObs[jj]->prn().system() == sys) {
855 satNum++;
856 }
857 }
858 // all LCs
859 unsigned realUsedLCs = OPT->LCs(sys).size();
860 // exclude pseudo obs GIM
861 if (OPT->_pseudoObsIono && !pseudoObsIono) {
862 realUsedLCs -= 1;
863 }
864
865 trafoObs += satNum * realUsedLCs;
866
867 if (OPT->_pseudoObsIono && pseudoObsIono) {
868 trafoObs -= 1; // pseudo obs iono with respect to refSat
869 }
870 }
871 _datumTrafo->setNumObs(trafoObs);
872 _datumTrafo->setNumPar(_parlist.nPar());
873 _datumTrafo->initAA();
874}
875
876//
877//////////////////////////////////////////////////////////////////////////////
878bool t_pppFilter::resetRefSatellitesLastEpoch(
879 std::vector<t_pppSatObs*> &obsVector,
880 const QMap<char, t_pppRefSat*> &refSatMap,
881 const QMap<char, t_pppRefSat*> &refSatMapLastEpoch) {
882 bool resetRefSat;
883 // reference satellite definition per system
884 const QList<char> &usedSystems = _parlist.usedSystems();
885 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
886 resetRefSat = false;
887 char sys = usedSystems[iSys];
888 t_prn newPrn = refSatMap[sys]->prn();
889 t_prn oldPrn = refSatMapLastEpoch[sys]->prn();
890#ifdef BNC_DEBUG_PPP
891 if (oldPrn != newPrn) {
892 LOG << "oldRef: " << oldPrn.toString() << " => newRef " << newPrn.toString() << endl;
893 }
894#endif
895 vector<t_pppSatObs*>::iterator it = obsVector.begin();
896 while (it != obsVector.end()) {
897 t_pppSatObs *satObs = *it;
898 if (satObs->prn() == newPrn) {
899 resetRefSat = true;
900 satObs->setAsReference();
901 } else if (satObs->prn() == oldPrn) {
902 satObs->resetReference();
903 }
904 it++;
905 }
906 if (!resetRefSat) {
907 _obsPool->setRefSatChangeRequired(sys, true);
908 return resetRefSat;
909 }
910 }
911 return resetRefSat;
912}
Note: See TracBrowser for help on using the repository browser.