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

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