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

Last change on this file since 10031 was 10031, 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(int num) {
51 _numSat = 0;
52 const double maxSolGap = 60.0;
53 bool setNeuNoiseToZero = false;
54
55 if (num > 1) {
56 setNeuNoiseToZero = true;
57 }
58
59 // Vector of all Observations
60 // --------------------------
61 t_pppObsPool::t_epoch *epoch = _obsPool->lastEpoch();
62 if (!epoch) {
63 return failure;
64 }
65 vector<t_pppSatObs*> &allObs = epoch->obsVector();
66
67 // Time of the Epoch
68 // -----------------
69 _epoTime = epoch->epoTime();
70
71 if (!_firstEpoTime.valid() ||
72 !_lastEpoTimeOK.valid()||
73 (maxSolGap > 0.0 && _epoTime - _lastEpoTimeOK > maxSolGap)) {
74 _firstEpoTime = _epoTime;
75 }
76
77 string epoTimeStr = string(_epoTime);
78
79 const QMap<char, t_pppRefSat*> &refSatMap = epoch->refSatMap();
80
81 const QList<char> &usedSystems = _parlist.usedSystems();
82 //--
83 // Set Parameters
84 if (_parlist.set(_epoTime, allObs, refSatMap) != success) {
85 return failure;
86 }
87
88#ifdef BNC_DEBUG_PPP
89 if (OPT->_obsModelType == OPT->DCMcodeBias ||
90 OPT->_obsModelType == OPT->DCMphaseBias) {
91// _parlist.printParams(_epoTime);
92 }
93#endif
94
95 // Status Vector, Variance-Covariance Matrix
96 // -----------------------------------------
97 ColumnVector xFltOld = _xFlt;
98 SymmetricMatrix QFltOld = _QFlt;
99 setStateVectorAndVarCovMatrix(xFltOld, QFltOld, setNeuNoiseToZero);
100
101 // Pre-Process Satellite Systems separately
102 // ----------------------------------------
103 bool preProcessing = false;
104 if (OPT->_obsModelType == OPT->DCMcodeBias ||
105 OPT->_obsModelType == OPT->DCMphaseBias) {
106 preProcessing = true;
107 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
108 char sys = usedSystems[iSys];
109 _refPrn.set(sys, 0);
110 if (OPT->_refSatRequired) {
111 _refPrn = refSatMap[sys]->prn();
112 }
113 vector<t_pppSatObs*> obsVector;
114 for (unsigned jj = 0; jj < allObs.size(); jj++) {
115 if (allObs[jj]->prn().system() == sys) {
116 obsVector.push_back(allObs[jj]);
117 }
118 }
119 if (iSys == 0) {
120 _datumTrafo->setFirstSystem(sys);
121 }
122 if (processSystem(OPT->LCs(sys), obsVector, _refPrn,
123 epoch->pseudoObsIono(), preProcessing) != success) {
124 LOG << sys << ": processSystem != success (pre-processing)" << endl;
125 _xFlt = xFltOld;
126 _QFlt = QFltOld;
127 restoreState(1);
128 return failure;
129 }
130 }
131 // refSat change required?
132 // -----------------------
133 if (_obsPool->refSatChangeRequired()) {
134 _xFlt = xFltOld;
135 _QFlt = QFltOld;
136 return success;
137 } else if (!_obsPool->refSatChangeRequired()) {
138 initDatumTransformation(allObs, epoch->pseudoObsIono());
139 }
140 }
141
142 // Process Satellite Systems separately
143 // ------------------------------------
144 preProcessing = false;
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
163 << endl;
164 if (processSystem(OPT->LCs(sys), obsVector, _refPrn,
165 epoch->pseudoObsIono(), preProcessing) != success) {
166 LOG << "processSystem != success (fin-processing)" << endl;
167 if (OPT->_obsModelType == OPT->DCMcodeBias ||
168 OPT->_obsModelType == OPT->DCMphaseBias) {
169 _xFlt = xFltOld;
170 _QFlt = QFltOld;
171 restoreState(2);
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, const t_prn &refPrn,
189 bool pseudoObsIonoAvailable, bool preProcessing) {
190 LOG.setf(ios::fixed);
191 char sys = refPrn.system();
192
193 // Detect Cycle Slips
194 // ------------------
195 if (detectCycleSlips(LCs, obsVector, refPrn, preProcessing) != success) {
196 return failure;
197 }
198 if (preProcessing && _obsPool->refSatChangeRequired(sys)) {
199 return success;
200 }
201
202 ColumnVector xSav = _xFlt;
203 SymmetricMatrix QSav = _QFlt;
204 string epoTimeStr = string(_epoTime);
205 const vector<t_pppParam*> &params = _parlist.params();
206 unsigned nPar = _parlist.nPar();
207
208 unsigned usedLCs = LCs.size();
209 if (OPT->_pseudoObsIono && !pseudoObsIonoAvailable) {
210 usedLCs -= 1; // GIM not used
211 }
212 // max Obs
213 unsigned maxObs = obsVector.size() * usedLCs;
214
215 if (OPT->_pseudoObsIono && pseudoObsIonoAvailable) {
216 maxObs -= 1; // pseudo obs iono with respect to refSat
217 }
218
219 // Outlier Detection Loop
220 // ----------------------
221 for (unsigned iOutlier = 0; iOutlier < maxObs; iOutlier++) {
222
223 if (iOutlier > 0) {
224 _xFlt = xSav;
225 _QFlt = QSav;
226 }
227
228 // First-Design Matrix, Terms Observed-Computed, Weight Matrix
229 // -----------------------------------------------------------
230 Matrix AA(maxObs, nPar);
231 ColumnVector ll(maxObs);
232 DiagonalMatrix PP(maxObs);
233 PP = 0.0;
234
235 int iObs = -1;
236 vector<t_pppSatObs*> usedObs;
237 vector<t_lc::type> usedTypes;
238
239 // Real Observations
240 // =================
241 double nSat = 0;
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 nSat++;
249 for (unsigned jj = 0; jj < usedLCs; jj++) {
250 const t_lc::type tLC = LCs[jj];
251 if (tLC == t_lc::GIM) {
252 continue;
253 }
254 ++iObs;
255 usedObs.push_back(obs);
256 usedTypes.push_back(tLC);
257 for (unsigned iPar = 0; iPar < nPar; iPar++) {
258 const t_pppParam *par = params[iPar];
259 AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
260 }
261 ll[iObs] = obs->obsValue(tLC) - obs->cmpValue(tLC)
262 - DotProduct(_x0, AA.Row(iObs + 1));
263 PP[iObs] = 1.0 / (obs->sigma(tLC) * obs->sigma(tLC));
264 }
265 }
266 }
267
268 // Check number of observations
269 // ----------------------------
270 if ((iObs +1) < OPT->_minObs) {
271 LOG << "t_pppFilter::processSystem not enough observations " << sys << ": " << iObs + 1 << "\n";
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 LOG << epoTimeStr << " Outlier ("
353 << ((preProcessing) ? "pre-processing) " : "fin-processing) ")
354 << t_lc::toString(maxOutlierLC) << ' ' << obs->prn().toString()
355 << ' ' << setw(8) << setprecision(4) << maxOutlier << endl;
356 _obsPool->setRefSatChangeRequired(sys, true);
357 break;
358 }
359 else {
360 obs->setOutlier();
361 }
362 }
363 else { // fin-processing
364 LOG << epoTimeStr << " Outlier " << t_lc::toString(maxOutlierLC) << ' '
365 << obs->prn().toString() << ' ' << setw(8) << setprecision(4)
366 << maxOutlier << endl;
367 if (par) {
368 if (par->ambResetCandidate() ||
369 OPT->_obsModelType == OPT->DCMcodeBias ||
370 OPT->_obsModelType == OPT->DCMphaseBias) {
371 resetAmb(par->prn(), obsVector, maxOutlierLC, &QSav, &xSav);
372 }
373 else {
374 par->setAmbResetCandidate();
375 obs->setOutlier();
376 }
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 = 50.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, tLC);
464 }
465 }
466 // Check Pre-Fit Residuals
467 // -----------------------
468 else {
469 if (!preProcessing) {
470 ColumnVector AA(nPar);
471 for (unsigned iPar = 0; iPar < nPar; iPar++) {
472 const t_pppParam *par = params[iPar];
473 AA[iPar] = par->partial(_epoTime, obs, tLC, refPrn);
474 }
475 double ll = obs->obsValue(tLC) - obs->cmpValue(tLC)
476 - DotProduct(_x0, AA);
477 double vv = DotProduct(AA, _xFlt) - ll;
478 if (fabs(vv) > SLIP) {
479 LOG << epoTimeStr << " cycle slip detected " << t_lc::toString(tLC)
480 << ' ' << obs->prn().toString() << ' ' << setw(8)
481 << setprecision(4) << vv << endl;
482 resetAmb(obs->prn(), obsVector, tLC);
483 }
484 }
485 }
486 }
487 }
488 }
489 return success;
490}
491
492// Reset Ambiguity Parameter (cycle slip)
493////////////////////////////////////////////////////////////////////////////
494t_irc t_pppFilter::resetAmb(t_prn prn, const vector<t_pppSatObs*> &obsVector,
495 t_lc::type lc, SymmetricMatrix *QSav, ColumnVector *xSav) {
496
497 t_irc irc = failure;
498 vector<t_pppParam*> &params = _parlist.params();
499 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
500 t_pppParam *par = params[iPar];
501 if (par->type() == t_pppParam::amb && par->prn() == prn) {
502 int ind = par->indexNew();
503 t_lc::type tLC = par->tLC();
504 if (tLC != lc) {
505 continue;
506 }
507 LOG << string(_epoTime) << " RESET " << par->toString() << endl;
508 delete par;
509 par = new t_pppParam(t_pppParam::amb, prn, tLC, &obsVector);
510 par->setIndex(ind);
511 params[iPar] = par;
512 for (unsigned ii = 1; ii <= params.size(); ii++) {
513 _QFlt(ii, ind + 1) = 0.0;
514 if (QSav) {
515 (*QSav)(ii, ind + 1) = 0.0;
516 }
517 }
518 _QFlt(ind + 1, ind + 1) = par->sigma0() * par->sigma0();
519 if (QSav) {
520 (*QSav)(ind + 1, ind + 1) = _QFlt(ind + 1, ind + 1);
521 }
522 _xFlt[ind] = 0.0;
523 if (xSav) {
524 (*xSav)[ind] = _xFlt[ind];
525 }
526 _x0[ind] = par->x0();
527 irc = success;
528 }
529 }
530
531 return irc;
532}
533
534// Add infinite noise to iono
535////////////////////////////////////////////////////////////////////////////
536t_irc t_pppFilter::addNoiseToPar(t_pppParam::e_type parType, char sys) {
537 t_irc irc = failure;
538 vector<t_pppParam*> &params = _parlist.params();
539 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
540 t_pppParam *par = params[iPar];
541 if (par->type() == parType && par->prn().system() == sys) {
542 int ind = par->indexNew();
543 LOG << string(_epoTime) << " ADD NOISE TO " << par->toString() << endl;
544 par->setIndex(ind);
545 _QFlt(ind + 1, ind + 1) = par->sigma0() * par->sigma0();
546 irc = success;
547 }
548 }
549
550 return irc;
551}
552
553// Compute various DOP Values
554////////////////////////////////////////////////////////////////////////////
555void t_pppFilter::cmpDOP(const vector<t_pppSatObs*> &obsVector,
556 const QMap<char, t_pppRefSat*> &refSatMap) {
557
558 _dop.reset();
559
560 try {
561 const unsigned numPar = 4;
562 Matrix AA(obsVector.size(), numPar);
563 _numSat = 0;
564 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
565 t_pppSatObs *obs = obsVector[ii];
566 char sys = obs->prn().system();
567 t_prn refPrn = t_prn();
568 if (OPT->_refSatRequired) {
569 refPrn = refSatMap[sys]->prn();
570 }
571 if (obs->isValid() && !obs->outlier()) {
572 ++_numSat;
573 for (unsigned iPar = 0; iPar < numPar; iPar++) {
574 const t_pppParam *par = _parlist.params()[iPar];
575 AA[_numSat - 1][iPar] = par->partial(_epoTime, obs, t_lc::c1, refPrn);
576 }
577 }
578 }
579 if (_numSat < 4) {
580 return;
581 }
582 AA = AA.Rows(1, _numSat);
583 SymmetricMatrix NN;
584 NN << AA.t() * AA;
585 SymmetricMatrix QQ = NN.i();
586
587 _dop.H = sqrt(QQ(1, 1) + QQ(2, 2));
588 _dop.V = sqrt(QQ(3, 3));
589 _dop.P = sqrt(QQ(1, 1) + QQ(2, 2) + QQ(3, 3));
590 _dop.T = sqrt(QQ(4, 4));
591 _dop.G = sqrt(QQ(1, 1) + QQ(2, 2) + QQ(3, 3) + QQ(4, 4));
592 } catch (...) {
593 }
594}
595
596//
597////////////////////////////////////////////////////////////////////////////
598void t_pppFilter::predictCovCrdPart(const SymmetricMatrix &QFltOld, bool setNeuNoiseToZero) {
599
600 const vector<t_pppParam*> &params = _parlist.params();
601 if (params.size() < 3) {
602 return;
603 }
604
605 bool first = (params[0]->indexOld() < 0);
606
607 SymmetricMatrix Qneu(3);
608 Qneu = 0.0;
609 for (unsigned ii = 0; ii < 3; ii++) {
610 const t_pppParam *par = params[ii];
611 if (first) {
612 Qneu[ii][ii] = par->sigma0() * par->sigma0();
613 } else {
614 Qneu[ii][ii] = par->noise() * par->noise();
615 }
616 }
617
618 const t_pppStation *sta = PPP_CLIENT->staRover();
619 SymmetricMatrix Qxyz(3);
620 covariNEU_XYZ(Qneu, sta->ellApr().data(), Qxyz);
621
622 if (first) {
623 _QFlt.SymSubMatrix(1, 3) = Qxyz;
624 } else {
625 double dt = _epoTime - _firstEpoTime;
626 if (dt < OPT->_seedingTime || setNeuNoiseToZero) {
627 _QFlt.SymSubMatrix(1, 3) = QFltOld.SymSubMatrix(1, 3);
628 } else {
629 _QFlt.SymSubMatrix(1, 3) = QFltOld.SymSubMatrix(1, 3) + Qxyz;
630 }
631 }
632}
633
634//
635////////////////////////////////////////////////////////////////////////////
636void t_pppFilter::setStateVectorAndVarCovMatrix(const ColumnVector &xFltOld,
637 const SymmetricMatrix &QFltOld, bool setNeuNoiseToZero) {
638
639 const vector<t_pppParam*> &params = _parlist.params();
640 unsigned nPar = params.size();
641
642 _QFlt.ReSize(nPar);
643 _QFlt = 0.0;
644 _xFlt.ReSize(nPar);
645 _xFlt = 0.0;
646 _x0.ReSize(nPar);
647 _x0 = 0.0;
648
649 for (unsigned ii = 0; ii < nPar; ii++) {
650 t_pppParam *par1 = params[ii];
651 if (QFltOld.size() == 0) {
652 par1->resetIndex();
653 }
654 _x0[ii] = par1->x0();
655 int iOld = par1->indexOld();
656 if (iOld < 0) {
657 _QFlt[ii][ii] = par1->sigma0() * par1->sigma0(); // new parameter
658 } else {
659 _QFlt[ii][ii] = QFltOld[iOld][iOld] + par1->noise() * par1->noise();
660 _xFlt[ii] = xFltOld[iOld];
661 for (unsigned jj = 0; jj < ii; jj++) {
662 t_pppParam *par2 = params[jj];
663 int jOld = par2->indexOld();
664 if (jOld >= 0) {
665 _QFlt[ii][jj] = QFltOld(iOld + 1, jOld + 1);
666 }
667 }
668 }
669 }
670 predictCovCrdPart(QFltOld, setNeuNoiseToZero);
671}
672
673// Compute datum transformation
674////////////////////////////////////////////////////////////////////////////
675t_irc t_pppFilter::datumTransformation(
676 const QMap<char, t_pppRefSat*> &refSatMap) {
677
678 // get last epoch
679 t_pppObsPool::t_epoch *epoch = _obsPool->lastEpoch();
680#ifdef BNC_DEBUG_PPP
681 LOG << "GET LAST EPOCH" << endl;
682#endif
683 if (!epoch) {
684 LOG << "t_pppFilter::datumTransformation: !lastEpoch" << endl;
685 return failure;
686 }
687 _epoTime = epoch->epoTime();
688
689 LOG.setf(ios::fixed);
690 LOG << "DATUM TRANSFORMATION in Epoch "<< string(_epoTime) << endl;
691
692 vector<t_pppSatObs*> &allObs = epoch->obsVector();
693
694 const QMap<char, t_pppRefSat*> &refSatMapLastEpoch = epoch->refSatMap();
695
696 bool pseudoObsIono = epoch->pseudoObsIono();
697
698 // reset old and set new refSats in last epoch (ambiguities/GIM)
699 // =============================================================
700 if (resetRefSatellitesLastEpoch(allObs, refSatMap, refSatMapLastEpoch) != true) {
701 LOG << "datumTransformation: resetRefSatellitesLastEpoch != success" << endl;
702 return failure;
703 }
704
705 if (OPT->_obsModelType == OPT->UncombPPP) {
706 _obsPool->putEpoch(_epoTime, allObs, pseudoObsIono, refSatMap);
707 return success;
708 }
709
710 // set AA2
711 // =======
712 if (_parlist.set(_epoTime, allObs, refSatMap) != success) {
713 return failure;
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 allObs[jj]->resetOutlier();
727 obsVector.push_back(allObs[jj]);
728 }
729 }
730 if (iSys == 0) {
731 _datumTrafo->setFirstSystem(sys);
732 }
733 vector<t_lc::type> LCs = OPT->LCs(sys);
734 unsigned usedLCs = LCs.size();
735 if (OPT->_pseudoObsIono && !pseudoObsIono) {
736 usedLCs -= 1; // GIM not used
737 }
738 // max Obs
739 unsigned maxObs = obsVector.size() * usedLCs;
740
741 if (OPT->_pseudoObsIono && pseudoObsIono) {
742 maxObs -= 1; // pseudo obs iono with respect to refSat
743 }
744
745 const vector<t_pppParam*> &params = _parlist.params();
746 unsigned nPar = _parlist.nPar();
747
748 Matrix AA(maxObs, nPar); AA = 0.0;
749
750 // Real Observations
751 // -----------------
752 int iObs = -1;
753 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
754 t_pppSatObs *obs = obsVector[ii];
755 for (unsigned jj = 0; jj < usedLCs; jj++) {
756 const t_lc::type tLC = LCs[jj];
757 if (tLC == t_lc::GIM) {
758 continue;
759 }
760 ++iObs;
761 for (unsigned iPar = 0; iPar < nPar; iPar++) {
762 const t_pppParam *par = params[iPar];
763 AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
764 }
765 }
766 }
767
768 if (!(iObs+1)) {
769 continue;
770 }
771 _datumTrafo->updateIndices(sys, iObs + 1);
772
773 if (_datumTrafo->prepareAA(AA.SubMatrix(1, iObs + 1, 1, nPar), 2) != success) {
774 return failure;
775 }
776 }
777 _datumTrafo->updateNumObs();
778
779 // Datum Transformation
780 // ====================
781 if (_datumTrafo->computeTrafoMatrix() != success) {
782 return failure;
783 }
784
785 ColumnVector xFltOld = _xFlt;
786 SymmetricMatrix QFltOld = _QFlt;
787
788 _QFlt << _datumTrafo->D21() * QFltOld * _datumTrafo->D21().t();
789 _xFlt = _datumTrafo->D21() * xFltOld;
790
791#ifdef BNC_DEBUG_PPP
792// LOG << "xFltOld:\n" << xFltOld << endl;
793// LOG << "xFlt :\n" << _xFlt << endl;
794#endif
795
796 // Reset Ambiguities after Datum Transformation
797 // ============================================
798 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
799 char sys = usedSystems[iSys];
800 vector<t_lc::type> LCs = OPT->LCs(sys);
801 unsigned usedLCs = LCs.size();
802 if (OPT->_pseudoObsIono && !pseudoObsIono) {
803 usedLCs -= 1; // GIM not used
804 }
805 t_prn refPrnOld = refSatMapLastEpoch[sys]->prn();
806 t_prn refPrnNew = refSatMap[sys]->prn();
807 if (refPrnNew != refPrnOld) {
808 for (unsigned jj = 0; jj < usedLCs; jj++) {
809 const t_lc::type tLC = LCs[jj];
810 if (tLC == t_lc::GIM) {
811 continue;
812 }
813 resetAmb(refPrnOld, allObs, tLC);
814 }
815 }
816 }
817
818 // switch AA2 to AA1
819 // =================
820 _datumTrafo->switchAA();
821
822 _obsPool->putEpoch(_epoTime, allObs, pseudoObsIono, refSatMap);
823#ifdef BNC_DEBUG_PPP
824 LOG << "PUT EPOCH t_pppFilter::datumTransformation " << _epoTime.timestr().c_str() << endl;
825#endif
826
827 return success;
828}
829
830// Init datum transformation
831////////////////////////////////////////////////////////////////////////////
832void t_pppFilter::initDatumTransformation(
833 const std::vector<t_pppSatObs*> &allObs, bool pseudoObsIono) {
834 unsigned trafoObs = 0;
835 const QList<char> &usedSystems = _parlist.usedSystems();
836 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
837 char sys = usedSystems[iSys];
838 int satNum = 0;
839 for (unsigned jj = 0; jj < allObs.size(); jj++) {
840 if (allObs[jj]->prn().system() == sys) {
841 satNum++;
842 }
843 }
844 // all LCs
845 unsigned realUsedLCs = OPT->LCs(sys).size();
846 // exclude pseudo obs GIM
847 if (OPT->_pseudoObsIono && !pseudoObsIono) {
848 realUsedLCs -= 1;
849 }
850
851 trafoObs += satNum * realUsedLCs;
852
853 if (OPT->_pseudoObsIono && pseudoObsIono) {
854 trafoObs -= 1; // pseudo obs iono with respect to refSat
855 }
856 }
857 _datumTrafo->setNumObs(trafoObs);
858 _datumTrafo->setNumPar(_parlist.nPar());
859 _datumTrafo->initAA();
860}
861
862//
863//////////////////////////////////////////////////////////////////////////////
864bool t_pppFilter::resetRefSatellitesLastEpoch(
865 std::vector<t_pppSatObs*> &obsVector,
866 const QMap<char, t_pppRefSat*> &refSatMap,
867 const QMap<char, t_pppRefSat*> &refSatMapLastEpoch) {
868 bool resetRefSat;
869 // reference satellite definition per system
870 const QList<char> &usedSystems = _parlist.usedSystems();
871 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
872 resetRefSat = false;
873 char sys = usedSystems[iSys];
874 t_prn newPrn = refSatMap[sys]->prn();
875 t_prn oldPrn = refSatMapLastEpoch[sys]->prn();
876#ifdef BNC_DEBUG_PPP
877 if (oldPrn != newPrn) {
878 LOG << "oldRef: " << oldPrn.toString() << " => newRef " << newPrn.toString() << endl;
879 }
880#endif
881 vector<t_pppSatObs*>::iterator it = obsVector.begin();
882 while (it != obsVector.end()) {
883 t_pppSatObs *satObs = *it;
884 if (satObs->prn() == newPrn) {
885 resetRefSat = true;
886 satObs->setAsReference();
887 } else if (satObs->prn() == oldPrn) {
888 satObs->resetReference();
889 }
890 it++;
891 }
892 if (!resetRefSat) {
893 _obsPool->setRefSatChangeRequired(sys, true);
894 return resetRefSat;
895 }
896 }
897 return resetRefSat;
898}
Note: See TracBrowser for help on using the repository browser.