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

Last change on this file since 9978 was 9978, checked in by stuerze, 14 months ago

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