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

Last change on this file since 10011 was 10011, 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.2 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 const QList<char> &usedSystems = _parlist.usedSystems();
75 //--
76 // Set Parameters
77 if (_parlist.set(_epoTime, allObs, refSatMap) != success) {
78 return failure;
79 }
80#ifdef BNC_DEBUG_PPP
81 if (OPT->_obsModelType == OPT->DCMcodeBias ||
82 OPT->_obsModelType == OPT->DCMphaseBias) {
83 _parlist.printParams(_epoTime);
84 }
85#endif
86
87 // Status Vector, Variance-Covariance Matrix
88 // -----------------------------------------
89 ColumnVector xFltOld = _xFlt;
90 SymmetricMatrix QFltOld = _QFlt;
91 bool setNeuNoiseToZero = false;
92 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
93 char sys = usedSystems[iSys];
94 int num = 0;
95 for (unsigned jj = 0; jj < allObs.size(); jj++) {
96 if (allObs[jj]->prn().system() == sys) {
97 ++num;
98 }
99 }
100 if (num < OPT->_minObs) {
101 setNeuNoiseToZero = true;
102 }
103 }
104 setStateVectorAndVarCovMatrix(xFltOld, QFltOld, setNeuNoiseToZero);
105
106 // Pre-Process Satellite Systems separately
107 // ----------------------------------------
108 bool preProcessing = false;
109 if (OPT->_obsModelType == OPT->DCMcodeBias ||
110 OPT->_obsModelType == OPT->DCMphaseBias) {
111 preProcessing = true;
112 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
113 char sys = usedSystems[iSys];
114 _refPrn.set(sys, 0);
115 if (OPT->_refSatRequired) {
116 _refPrn = refSatMap[sys]->prn();
117 }
118 vector<t_pppSatObs*> obsVector;
119 for (unsigned jj = 0; jj < allObs.size(); jj++) {
120 if (allObs[jj]->prn().system() == sys) {
121 obsVector.push_back(allObs[jj]);
122 }
123 }
124 if (iSys == 0) {
125 _datumTrafo->setFirstSystem(sys);
126 }
127 if (processSystem(OPT->LCs(sys), obsVector, _refPrn,
128 epoch->pseudoObsIono(), preProcessing) != success) {
129 LOG << sys << ": processSystem != success (pre-processing)" << endl;
130 _xFlt = xFltOld;
131 _QFlt = QFltOld;
132 _obsPool->deleteLastEpoch();
133 restoreState(2);
134 return failure;
135 }
136 }
137 // refSat change required?
138 // -----------------------
139 if (_obsPool->refSatChangeRequired()) {
140 _xFlt = xFltOld;
141 _QFlt = QFltOld;
142 return success;
143 } else if (!_obsPool->refSatChangeRequired()) {
144 initDatumTransformation(allObs, epoch->pseudoObsIono());
145 }
146 }
147
148 // Process Satellite Systems separately
149 // ------------------------------------
150 preProcessing = false;
151 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
152 char sys = usedSystems[iSys];
153 _refPrn.set(sys, 0);
154 if (OPT->_refSatRequired) {
155 _refPrn = refSatMap[sys]->prn();
156 }
157 unsigned int num = 0;
158 vector<t_pppSatObs*> obsVector;
159 for (unsigned jj = 0; jj < allObs.size(); jj++) {
160 if (allObs[jj]->prn().system() == sys) {
161 obsVector.push_back(allObs[jj]);
162 ++num;
163 }
164 }
165 if (iSys == 0 && OPT->_obsModelType == OPT->UncombPPP) {
166 _datumTrafo->setFirstSystem(sys);
167 }
168 LOG << epoTimeStr << " SATNUM " << sys << ' ' << right << setw(2) << num
169 << endl;
170 if (processSystem(OPT->LCs(sys), obsVector, _refPrn, epoch->pseudoObsIono(),
171 preProcessing) != success) {
172 LOG << "processSystem != success (fin-processing)" << endl;
173 if (OPT->_obsModelType == OPT->DCMcodeBias ||
174 OPT->_obsModelType == OPT->DCMphaseBias) {
175 _xFlt = xFltOld;
176 _QFlt = QFltOld;
177 _obsPool->deleteLastEpoch();
178 restoreState(3);
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 LOG << epoTimeStr << " Outlier ("
363 << ((preProcessing) ? "pre-processing) " : "fin-processing) ")
364 << t_lc::toString(maxOutlierLC) << ' ' << obs->prn().toString()
365 << ' ' << setw(8) << setprecision(4) << maxOutlier << endl;
366 if (obs->prn() == refPrn) {
367 _obsPool->setRefSatChangeRequired(sys, true);
368 break;
369 } else {
370 obs->setOutlier();
371 }
372 } else { // fin-processing
373 LOG << epoTimeStr << " Outlier " << t_lc::toString(maxOutlierLC) << ' '
374 << obs->prn().toString() << ' ' << setw(8) << setprecision(4)
375 << maxOutlier << endl;
376 if (par) {
377 if (par->ambResetCandidate() ||
378 OPT->_obsModelType == OPT->DCMcodeBias ||
379 OPT->_obsModelType == OPT->DCMphaseBias) {
380 resetAmb(par->prn(), obsVector, maxOutlierLC, &QSav, &xSav);
381 }
382 else {
383 par->setAmbResetCandidate();
384 obs->setOutlier();
385 }
386 }
387 else {
388 obs->setOutlier();
389 }
390 }
391 }
392 // Print Residuals
393 // ---------------
394 else {
395 if (!preProcessing) {
396 for (unsigned jj = 0; jj < LCs.size(); jj++) {
397 for (unsigned ii = 0; ii < usedObs.size(); ii++) {
398 const t_lc::type tLC = usedTypes[ii];
399 t_pppSatObs *obs = usedObs[ii];
400 if (tLC == LCs[jj]) {
401 obs->setRes(tLC, vv[ii]);
402 LOG << epoTimeStr << " RES " << left << setw(3)
403 << t_lc::toString(tLC) << right << ' '
404 << obs->prn().toString();
405 LOG << setw(9) << setprecision(4) << vv[ii] << endl;
406 }
407 }
408 }
409 }
410 break;
411 }
412 }
413 return success;
414}
415
416// Cycle-Slip Detection
417////////////////////////////////////////////////////////////////////////////
418t_irc t_pppFilter::detectCycleSlips(const vector<t_lc::type> &LCs,
419 const vector<t_pppSatObs*> &obsVector, const t_prn &refPrn,
420 bool preProcessing) {
421 const double SLIP = 50.0;
422 char sys = refPrn.system();
423 string epoTimeStr = string(_epoTime);
424 const vector<t_pppParam*> &params = _parlist.params();
425 unsigned nPar = _parlist.nPar();
426
427 for (unsigned ii = 0; ii < LCs.size(); ii++) {
428 const t_lc::type &tLC = LCs[ii];
429 if (t_lc::includesPhase(tLC)) {
430 for (unsigned iObs = 0; iObs < obsVector.size(); iObs++) {
431 const t_pppSatObs *obs = obsVector[iObs];
432
433 // Check set Slips and Jump Counters
434 // ---------------------------------
435 bool slip = false;
436
437 // in pre-processing only the reference satellite has to be checked
438 if (preProcessing && obs->prn() != refPrn) {
439 continue;
440 }
441
442 if (obs->slip()) {
443 LOG << epoTimeStr << "cycle slip set (obs) " << obs->prn().toString()
444 << endl;
445 slip = true;
446 }
447
448 if (_slips[obs->prn()]._obsSlipCounter != -1
449 && _slips[obs->prn()]._obsSlipCounter != obs->slipCounter()) {
450 LOG << epoTimeStr << "cycle slip set (obsSlipCounter) "
451 << obs->prn().toString() << endl;
452 slip = true;
453 }
454 if (!preProcessing) {
455 _slips[obs->prn()]._obsSlipCounter = obs->slipCounter();
456 }
457 if (_slips[obs->prn()]._biasJumpCounter != -1
458 && _slips[obs->prn()]._biasJumpCounter != obs->biasJumpCounter()) {
459 LOG << epoTimeStr << "cycle slip set (biasJumpCounter) "
460 << obs->prn().toString() << endl;
461 slip = true;
462 }
463 if (!preProcessing) {
464 _slips[obs->prn()]._biasJumpCounter = obs->biasJumpCounter();
465 }
466 // Slip Set
467 // --------
468 if (slip) {
469 if (preProcessing) {
470 _obsPool->setRefSatChangeRequired(sys, true);
471 } else {
472 resetAmb(obs->prn(), obsVector, tLC);
473 }
474 }
475 // Check Pre-Fit Residuals
476 // -----------------------
477 else {
478 ColumnVector AA(nPar);
479 for (unsigned iPar = 0; iPar < nPar; iPar++) {
480 const t_pppParam *par = params[iPar];
481 AA[iPar] = par->partial(_epoTime, obs, tLC, refPrn);
482 }
483 double ll = obs->obsValue(tLC) - obs->cmpValue(tLC)
484 - DotProduct(_x0, AA);
485 double vv = DotProduct(AA, _xFlt) - ll;
486 if (fabs(vv) > SLIP) {
487 LOG << epoTimeStr << " cycle slip detected " << t_lc::toString(tLC)
488 << ' ' << obs->prn().toString() << ' ' << setw(8)
489 << setprecision(4) << vv << endl;
490 if (preProcessing) {
491 _obsPool->setRefSatChangeRequired(sys, true);
492 } else {
493 resetAmb(obs->prn(), obsVector, tLC);
494 }
495 }
496 }
497 }
498 }
499 }
500 return success;
501}
502
503// Reset Ambiguity Parameter (cycle slip)
504////////////////////////////////////////////////////////////////////////////
505t_irc t_pppFilter::resetAmb(t_prn prn, const vector<t_pppSatObs*> &obsVector,
506 t_lc::type lc, SymmetricMatrix *QSav, ColumnVector *xSav) {
507
508 t_irc irc = failure;
509 vector<t_pppParam*> &params = _parlist.params();
510 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
511 t_pppParam *par = params[iPar];
512 if (par->type() == t_pppParam::amb && par->prn() == prn) {
513 int ind = par->indexNew();
514 t_lc::type tLC = par->tLC();
515 if (tLC != lc) {continue;}
516 LOG << string(_epoTime) << " RESET " << par->toString() << endl;
517 delete par;
518 par = new t_pppParam(t_pppParam::amb, prn, tLC, &obsVector);
519 par->setIndex(ind);
520 params[iPar] = par;
521 for (unsigned ii = 1; ii <= params.size(); ii++) {
522 _QFlt(ii, ind + 1) = 0.0;
523 if (QSav) {
524 (*QSav)(ii, ind + 1) = 0.0;
525 }
526 }
527 _QFlt(ind + 1, ind + 1) = par->sigma0() * par->sigma0();
528 if (QSav) {
529 (*QSav)(ind + 1, ind + 1) = _QFlt(ind + 1, ind + 1);
530 }
531 _xFlt[ind] = 0.0;
532 if (xSav) {
533 (*xSav)[ind] = _xFlt[ind];
534 }
535 _x0[ind] = par->x0();
536 irc = success;
537 }
538 }
539
540 return irc;
541}
542
543// Add infinite noise to iono
544////////////////////////////////////////////////////////////////////////////
545t_irc t_pppFilter::addNoiseToPar(t_pppParam::e_type parType, char sys) {
546 t_irc irc = failure;
547 vector<t_pppParam*> &params = _parlist.params();
548 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
549 t_pppParam *par = params[iPar];
550 if (par->type() == parType && par->prn().system() == sys) {
551 int ind = par->indexNew();
552 LOG << string(_epoTime) << " ADD NOISE TO " << par->toString() << endl;
553 par->setIndex(ind);
554 _QFlt(ind + 1, ind + 1) = par->sigma0() * par->sigma0();
555 irc = success;
556 }
557 }
558
559 return irc;
560}
561
562// Compute various DOP Values
563////////////////////////////////////////////////////////////////////////////
564void t_pppFilter::cmpDOP(const vector<t_pppSatObs*> &obsVector,
565 const QMap<char, t_pppRefSat*> &refSatMap) {
566
567 _dop.reset();
568
569 try {
570 const unsigned numPar = 4;
571 Matrix AA(obsVector.size(), numPar);
572 _numSat = 0;
573 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
574 t_pppSatObs *obs = obsVector[ii];
575 char sys = obs->prn().system();
576 t_prn refPrn = t_prn();
577 if (OPT->_refSatRequired) {
578 refPrn = refSatMap[sys]->prn();
579 }
580 if (obs->isValid() && !obs->outlier()) {
581 ++_numSat;
582 for (unsigned iPar = 0; iPar < numPar; iPar++) {
583 const t_pppParam *par = _parlist.params()[iPar];
584 AA[_numSat - 1][iPar] = par->partial(_epoTime, obs, t_lc::c1, refPrn);
585 }
586 }
587 }
588 if (_numSat < 4) {
589 return;
590 }
591 AA = AA.Rows(1, _numSat);
592 SymmetricMatrix NN;
593 NN << AA.t() * AA;
594 SymmetricMatrix QQ = NN.i();
595
596 _dop.H = sqrt(QQ(1, 1) + QQ(2, 2));
597 _dop.V = sqrt(QQ(3, 3));
598 _dop.P = sqrt(QQ(1, 1) + QQ(2, 2) + QQ(3, 3));
599 _dop.T = sqrt(QQ(4, 4));
600 _dop.G = sqrt(QQ(1, 1) + QQ(2, 2) + QQ(3, 3) + QQ(4, 4));
601 } catch (...) {
602 }
603}
604
605//
606////////////////////////////////////////////////////////////////////////////
607void t_pppFilter::predictCovCrdPart(const SymmetricMatrix &QFltOld, bool setNeuNoiseToZero) {
608
609 const vector<t_pppParam*> &params = _parlist.params();
610 if (params.size() < 3) {
611 return;
612 }
613
614 bool first = (params[0]->indexOld() < 0);
615
616 SymmetricMatrix Qneu(3);
617 Qneu = 0.0;
618 for (unsigned ii = 0; ii < 3; ii++) {
619 const t_pppParam *par = params[ii];
620 if (first) {
621 Qneu[ii][ii] = par->sigma0() * par->sigma0();
622 } else {
623 Qneu[ii][ii] = par->noise() * par->noise();
624 }
625 }
626
627 const t_pppStation *sta = PPP_CLIENT->staRover();
628 SymmetricMatrix Qxyz(3);
629 covariNEU_XYZ(Qneu, sta->ellApr().data(), Qxyz);
630
631 if (first) {
632 _QFlt.SymSubMatrix(1, 3) = Qxyz;
633 } else {
634 double dt = _epoTime - _firstEpoTime;
635 if (dt < OPT->_seedingTime || setNeuNoiseToZero) {
636 _QFlt.SymSubMatrix(1, 3) = QFltOld.SymSubMatrix(1, 3);
637 } else {
638 _QFlt.SymSubMatrix(1, 3) = QFltOld.SymSubMatrix(1, 3) + Qxyz;
639 }
640 }
641}
642
643//
644////////////////////////////////////////////////////////////////////////////
645void t_pppFilter::setStateVectorAndVarCovMatrix(const ColumnVector &xFltOld,
646 const SymmetricMatrix &QFltOld, bool setNeuNoiseToZero) {
647
648 const vector<t_pppParam*> &params = _parlist.params();
649 unsigned nPar = params.size();
650
651 _QFlt.ReSize(nPar);
652 _QFlt = 0.0;
653 _xFlt.ReSize(nPar);
654 _xFlt = 0.0;
655 _x0.ReSize(nPar);
656 _x0 = 0.0;
657
658 for (unsigned ii = 0; ii < nPar; ii++) {
659 t_pppParam *par1 = params[ii];
660 if (QFltOld.size() == 0) {
661 par1->resetIndex();
662 }
663 _x0[ii] = par1->x0();
664 int iOld = par1->indexOld();
665 if (iOld < 0) {
666 _QFlt[ii][ii] = par1->sigma0() * par1->sigma0(); // new parameter
667 } else {
668 _QFlt[ii][ii] = QFltOld[iOld][iOld] + par1->noise() * par1->noise();
669 _xFlt[ii] = xFltOld[iOld];
670 for (unsigned jj = 0; jj < ii; jj++) {
671 t_pppParam *par2 = params[jj];
672 int jOld = par2->indexOld();
673 if (jOld >= 0) {
674 _QFlt[ii][jj] = QFltOld(iOld + 1, jOld + 1);
675 }
676 }
677 }
678 }
679 predictCovCrdPart(QFltOld, setNeuNoiseToZero);
680}
681
682// Compute datum transformation
683////////////////////////////////////////////////////////////////////////////
684t_irc t_pppFilter::datumTransformation(
685 const QMap<char, t_pppRefSat*> &refSatMap) {
686
687 // get last epoch
688 t_pppObsPool::t_epoch *epoch = _obsPool->lastEpoch();
689 if (!epoch) {
690 LOG << "t_pppFilter::datumTransformation: !lastEpoch" << endl;
691 return failure;
692 }
693 _epoTime = epoch->epoTime();
694 LOG.setf(ios::fixed);
695 LOG << string(_epoTime) << " DATUM TRANSFORMATION " << endl;
696
697 vector<t_pppSatObs*> &allObs = epoch->obsVector();
698
699 const QMap<char, t_pppRefSat*> &refSatMapLastEpoch = epoch->refSatMap();
700
701 bool pseudoObsIono = epoch->pseudoObsIono();
702
703 // reset old and set new refSats in last epoch (ambiguities/GIM)
704 // =============================================================
705 if (resetRefSatellitesLastEpoch(allObs, refSatMap, refSatMapLastEpoch) != true) {
706 LOG << "datumTransformation: resetRefSatellitesLastEpoch != success" << endl;
707 return failure;
708 }
709
710 if (OPT->_obsModelType == OPT->UncombPPP) {
711 _obsPool->putEpoch(_epoTime, allObs, pseudoObsIono, refSatMap);
712 return success;
713 }
714
715 // set AA2
716 // =======
717 if (_parlist.set(_epoTime, allObs, refSatMap) != success) {
718 return failure;
719 }
720
721#ifdef BNC_DEBUG_PPP
722 _parlist.printParams(_epoTime);
723#endif
724
725 const QList<char> &usedSystems = _parlist.usedSystems();
726 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
727 char sys = usedSystems[iSys];
728 t_prn refPrn = refSatMap[sys]->prn();
729 vector<t_pppSatObs*> obsVector;
730 for (unsigned jj = 0; jj < allObs.size(); jj++) {
731 if (allObs[jj]->prn().system() == sys) {
732 allObs[jj]->resetOutlier();
733 obsVector.push_back(allObs[jj]);
734 }
735 }
736 if (iSys == 0) {
737 _datumTrafo->setFirstSystem(sys);
738 }
739 vector<t_lc::type> LCs = OPT->LCs(sys);
740 unsigned usedLCs = LCs.size();
741 if (OPT->_pseudoObsIono && !pseudoObsIono) {
742 usedLCs -= 1; // GIM not used
743 }
744 // max Obs
745 unsigned maxObs = obsVector.size() * usedLCs;
746
747 if (OPT->_pseudoObsIono && pseudoObsIono) {
748 maxObs -= 1; // pseudo obs iono with respect to refSat
749 }
750
751 const vector<t_pppParam*> &params = _parlist.params();
752 unsigned nPar = _parlist.nPar();
753
754 Matrix AA(maxObs, nPar);
755
756 // Real Observations
757 // -----------------
758 int iObs = -1;
759 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
760 t_pppSatObs *obs = obsVector[ii];
761 for (unsigned jj = 0; jj < usedLCs; jj++) {
762 const t_lc::type tLC = LCs[jj];
763 if (tLC == t_lc::GIM) {
764 continue;
765 }
766 ++iObs;
767 for (unsigned iPar = 0; iPar < nPar; iPar++) {
768 const t_pppParam *par = params[iPar];
769 AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
770 }
771 }
772 }
773
774 if (!(iObs+1)) {
775 continue;
776 }
777 _datumTrafo->updateIndices(sys, iObs + 1);
778#ifdef BNC_DEBUG_PPP
779 LOG << "AA Ncols/Nrows: " << AA.Ncols() << "/" << AA.Nrows() << " nPar: " << nPar << endl;
780 LOG << "AA.SubMatrix(1 .. " << iObs+1 << " , 1 .. " << nPar << ")" << endl;
781#endif
782 if (_datumTrafo->prepareAA(AA.SubMatrix(1, iObs + 1, 1, nPar), 2) != success) {
783 return failure;
784 }
785 }
786 _datumTrafo->updateNumObs();
787
788 // Datum Transformation
789 // ====================
790 if (_datumTrafo->computeTrafoMatrix() != success) {
791 return failure;
792 }
793
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 vector<t_lc::type> LCs = OPT->LCs(sys);
810 unsigned usedLCs = LCs.size();
811 if (OPT->_pseudoObsIono && !pseudoObsIono) {
812 usedLCs -= 1; // GIM not used
813 }
814 t_prn refPrnOld = refSatMapLastEpoch[sys]->prn();
815 t_prn refPrnNew = refSatMap[sys]->prn();
816 if (refPrnNew != refPrnOld) {
817 for (unsigned jj = 0; jj < usedLCs; jj++) {
818 const t_lc::type tLC = LCs[jj];
819 if (tLC == t_lc::GIM) {
820 continue;
821 }
822 resetAmb(refPrnOld, allObs, tLC);
823 }
824 }
825 }
826
827 // switch AA2 to AA1
828 // =================
829 _datumTrafo->switchAA();
830
831 _obsPool->putEpoch(_epoTime, allObs, pseudoObsIono, refSatMap);
832
833 return success;
834}
835
836// Init datum transformation
837////////////////////////////////////////////////////////////////////////////
838void t_pppFilter::initDatumTransformation(
839 const std::vector<t_pppSatObs*> &allObs, bool pseudoObsIono) {
840 unsigned trafoObs = 0;
841 const QList<char> &usedSystems = _parlist.usedSystems();
842 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
843 char sys = usedSystems[iSys];
844 int satNum = 0;
845 for (unsigned jj = 0; jj < allObs.size(); jj++) {
846 if (allObs[jj]->prn().system() == sys) {
847 satNum++;
848 }
849 }
850 // all LCs
851 unsigned realUsedLCs = OPT->LCs(sys).size();
852 // exclude pseudo obs GIM
853 if (OPT->_pseudoObsIono && !pseudoObsIono) {
854 realUsedLCs -= 1;
855 }
856
857 trafoObs += satNum * realUsedLCs;
858
859 if (OPT->_pseudoObsIono && pseudoObsIono) {
860 trafoObs -= 1; // pseudo obs iono with respect to refSat
861 }
862 }
863 _datumTrafo->setNumObs(trafoObs);
864 _datumTrafo->setNumPar(_parlist.nPar());
865 _datumTrafo->initAA();
866}
867
868//
869//////////////////////////////////////////////////////////////////////////////
870bool t_pppFilter::resetRefSatellitesLastEpoch(
871 std::vector<t_pppSatObs*> &obsVector,
872 const QMap<char, t_pppRefSat*> &refSatMap,
873 const QMap<char, t_pppRefSat*> &refSatMapLastEpoch) {
874 bool resetRefSat;
875 // reference satellite definition per system
876 const QList<char> &usedSystems = _parlist.usedSystems();
877 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
878 resetRefSat = false;
879 char sys = usedSystems[iSys];
880 t_prn newPrn = refSatMap[sys]->prn();
881 t_prn oldPrn = refSatMapLastEpoch[sys]->prn();
882#ifdef BNC_DEBUG_PPP
883 if (oldPrn != newPrn) {
884 LOG << "oldRef: " << oldPrn.toString() << " => newRef " << newPrn.toString() << endl;
885 }
886#endif
887 vector<t_pppSatObs*>::iterator it = obsVector.begin();
888 while (it != obsVector.end()) {
889 t_pppSatObs *satObs = *it;
890 if (satObs->prn() == newPrn) {
891 resetRefSat = true;
892 satObs->setAsReference();
893 } else if (satObs->prn() == oldPrn) {
894 satObs->resetReference();
895 }
896 it++;
897 }
898 if (!resetRefSat) {
899 _obsPool->setRefSatChangeRequired(sys, true);
900 return resetRefSat;
901 }
902 }
903 return resetRefSat;
904}
Note: See TracBrowser for help on using the repository browser.