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

Last change on this file since 10022 was 10022, 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) < OPT->_minObs) {
278 LOG << "t_pppFilter::processSystem not enough observations " << sys << ": " << iObs + 1 << "\n";
279 return failure;
280 }
281
282 if ((!iOutlier) &&
283 (OPT->_obsModelType == OPT->DCMcodeBias ||
284 OPT->_obsModelType == OPT->DCMphaseBias) && (!preProcessing)) {
285 _datumTrafo->updateIndices(sys, iObs + 1);
286 _datumTrafo->prepareAA(AA.SubMatrix(1, iObs + 1, 1, _parlist.nPar()), 1);
287 }
288
289 // Pseudo Obs Iono
290 // ================
291 if (OPT->_pseudoObsIono && pseudoObsIonoAvailable) {
292 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
293 t_pppSatObs *obs = obsVector[ii];
294 if (!obs->outlier()) {
295 for (unsigned jj = 0; jj < usedLCs; jj++) {
296 const t_lc::type tLC = LCs[jj];
297 if (tLC == t_lc::GIM && !obs->isReference()) {
298 ++iObs;
299 } else {
300 continue;
301 }
302 usedObs.push_back(obs);
303 usedTypes.push_back(tLC);
304 for (unsigned iPar = 0; iPar < nPar; iPar++) {
305 const t_pppParam *par = params[iPar];
306 AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
307 }
308 ll[iObs] = obs->obsValue(tLC) - obs->cmpValue(tLC)
309 - DotProduct(_x0, AA.Row(iObs + 1));
310 PP[iObs] = 1.0 / (obs->sigma(tLC) * obs->sigma(tLC));
311 }
312 }
313 }
314 }
315
316 // Truncate matrices
317 // -----------------
318 AA = AA.Rows(1, iObs + 1);
319 ll = ll.Rows(1, iObs + 1);
320 PP = PP.SymSubMatrix(1, iObs + 1);
321
322 // Kalman update step
323 // ------------------
324 kalman(AA, ll, PP, _QFlt, _xFlt);
325
326 // Check Residuals
327 // ---------------
328 ColumnVector vv = AA * _xFlt - ll;
329 double maxOutlier = 0.0;
330 int maxOutlierIndex = -1;
331 t_lc::type maxOutlierLC = t_lc::dummy;
332 for (unsigned ii = 0; ii < usedObs.size(); ii++) {
333 const t_lc::type tLC = usedTypes[ii];
334 double res = fabs(vv[ii]);
335 if (res > usedObs[ii]->maxRes(tLC)) {
336 if (res > fabs(maxOutlier)) {
337 maxOutlier = vv[ii];
338 maxOutlierIndex = ii;
339 maxOutlierLC = tLC;
340 }
341 }
342 }
343
344 // Mark outlier or break outlier detection loop
345 // --------------------------------------------
346 if (maxOutlierIndex > -1) {
347 t_pppSatObs *obs = usedObs[maxOutlierIndex];
348 t_pppParam *par = 0;
349 for (unsigned iPar = 0; iPar < nPar; iPar++) {
350 t_pppParam *hlp = params[iPar];
351 if (hlp->type() == t_pppParam::amb &&
352 hlp->prn() == obs->prn() &&
353 hlp->tLC() == usedTypes[maxOutlierIndex]) {
354 par = hlp;
355 }
356 }
357 if (preProcessing) {
358 if (obs->prn() == refPrn) {
359 LOG << epoTimeStr << " Outlier ("
360 << ((preProcessing) ? "pre-processing) " : "fin-processing) ")
361 << t_lc::toString(maxOutlierLC) << ' ' << obs->prn().toString()
362 << ' ' << setw(8) << setprecision(4) << maxOutlier << endl;
363 _obsPool->setRefSatChangeRequired(sys, true);
364 break;
365 }
366 else {
367 obs->setOutlier();
368 }
369 }
370 else { // fin-processing
371 LOG << epoTimeStr << " Outlier " << t_lc::toString(maxOutlierLC) << ' '
372 << obs->prn().toString() << ' ' << setw(8) << setprecision(4)
373 << maxOutlier << endl;
374 if (par) {
375 if (par->ambResetCandidate() ||
376 OPT->_obsModelType == OPT->DCMcodeBias ||
377 OPT->_obsModelType == OPT->DCMphaseBias) {
378 resetAmb(par->prn(), obsVector, maxOutlierLC, &QSav, &xSav);
379 }
380 else {
381 par->setAmbResetCandidate();
382 obs->setOutlier();
383 }
384 }
385 else {
386 obs->setOutlier();
387 }
388 }
389 }
390 // Print Residuals
391 // ---------------
392 else {
393 if (!preProcessing) {
394 for (unsigned jj = 0; jj < LCs.size(); jj++) {
395 for (unsigned ii = 0; ii < usedObs.size(); ii++) {
396 const t_lc::type tLC = usedTypes[ii];
397 t_pppSatObs *obs = usedObs[ii];
398 if (tLC == LCs[jj]) {
399 obs->setRes(tLC, vv[ii]);
400 LOG << epoTimeStr << " RES " << left << setw(3)
401 << t_lc::toString(tLC) << right << ' '
402 << obs->prn().toString();
403 LOG << setw(9) << setprecision(4) << vv[ii] << endl;
404 }
405 }
406 }
407 }
408 break;
409 }
410 }
411 return success;
412}
413
414// Cycle-Slip Detection
415////////////////////////////////////////////////////////////////////////////
416t_irc t_pppFilter::detectCycleSlips(const vector<t_lc::type> &LCs,
417 const vector<t_pppSatObs*> &obsVector, const t_prn &refPrn,
418 bool preProcessing) {
419 const double SLIP = 50.0;
420 char sys = refPrn.system();
421 string epoTimeStr = string(_epoTime);
422 const vector<t_pppParam*> &params = _parlist.params();
423 unsigned nPar = _parlist.nPar();
424
425 for (unsigned ii = 0; ii < LCs.size(); ii++) {
426 const t_lc::type &tLC = LCs[ii];
427 if (t_lc::includesPhase(tLC)) {
428 for (unsigned iObs = 0; iObs < obsVector.size(); iObs++) {
429 const t_pppSatObs *obs = obsVector[iObs];
430
431 // Check set Slips and Jump Counters
432 // ---------------------------------
433 bool slip = false;
434
435 // in pre-processing only the reference satellite has to be checked
436 if (preProcessing && obs->prn() != refPrn) {
437 continue;
438 }
439
440 if (obs->slip()) {
441 LOG << epoTimeStr << "cycle slip set (obs) " << obs->prn().toString()
442 << endl;
443 slip = true;
444 }
445
446 if (_slips[obs->prn()]._obsSlipCounter != -1
447 && _slips[obs->prn()]._obsSlipCounter != obs->slipCounter()) {
448 LOG << epoTimeStr << "cycle slip set (obsSlipCounter) "
449 << obs->prn().toString() << endl;
450 slip = true;
451 }
452 if (!preProcessing) {
453 _slips[obs->prn()]._obsSlipCounter = obs->slipCounter();
454 }
455 if (_slips[obs->prn()]._biasJumpCounter != -1
456 && _slips[obs->prn()]._biasJumpCounter != obs->biasJumpCounter()) {
457 LOG << epoTimeStr << "cycle slip set (biasJumpCounter) "
458 << obs->prn().toString() << endl;
459 slip = true;
460 }
461 if (!preProcessing) {
462 _slips[obs->prn()]._biasJumpCounter = obs->biasJumpCounter();
463 }
464 // Slip Set
465 // --------
466 if (slip) {
467 if (preProcessing) {
468 _obsPool->setRefSatChangeRequired(sys, true);
469 } else {
470 resetAmb(obs->prn(), obsVector, tLC);
471 }
472 }
473 // Check Pre-Fit Residuals
474 // -----------------------
475 else {
476 ColumnVector AA(nPar);
477 for (unsigned iPar = 0; iPar < nPar; iPar++) {
478 const t_pppParam *par = params[iPar];
479 AA[iPar] = par->partial(_epoTime, obs, tLC, refPrn);
480 }
481 double ll = obs->obsValue(tLC) - obs->cmpValue(tLC)
482 - DotProduct(_x0, AA);
483 double vv = DotProduct(AA, _xFlt) - ll;
484 if (fabs(vv) > SLIP) {
485 LOG << epoTimeStr << " cycle slip detected " << t_lc::toString(tLC)
486 << ' ' << obs->prn().toString() << ' ' << setw(8)
487 << setprecision(4) << vv << endl;
488 if (preProcessing) {
489 _obsPool->setRefSatChangeRequired(sys, true);
490 } else {
491 resetAmb(obs->prn(), obsVector, tLC);
492 }
493 }
494 }
495 }
496 }
497 }
498 return success;
499}
500
501// Reset Ambiguity Parameter (cycle slip)
502////////////////////////////////////////////////////////////////////////////
503t_irc t_pppFilter::resetAmb(t_prn prn, const vector<t_pppSatObs*> &obsVector,
504 t_lc::type lc, SymmetricMatrix *QSav, ColumnVector *xSav) {
505
506 t_irc irc = failure;
507 vector<t_pppParam*> &params = _parlist.params();
508 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
509 t_pppParam *par = params[iPar];
510 if (par->type() == t_pppParam::amb && par->prn() == prn) {
511 int ind = par->indexNew();
512 t_lc::type tLC = par->tLC();
513 if (tLC != lc) {
514 continue;
515 }
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) << "\nDATUM 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#ifdef BNC_DEBUG_PPP
721 //_parlist.printParams(_epoTime);
722#endif
723
724 const QList<char> &usedSystems = _parlist.usedSystems();
725 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
726 char sys = usedSystems[iSys];
727 t_prn refPrn = refSatMap[sys]->prn();
728 vector<t_pppSatObs*> obsVector;
729 for (unsigned jj = 0; jj < allObs.size(); jj++) {
730 if (allObs[jj]->prn().system() == sys) {
731 allObs[jj]->resetOutlier();
732 obsVector.push_back(allObs[jj]);
733 }
734 }
735 if (iSys == 0) {
736 _datumTrafo->setFirstSystem(sys);
737 }
738 vector<t_lc::type> LCs = OPT->LCs(sys);
739 unsigned usedLCs = LCs.size();
740 if (OPT->_pseudoObsIono && !pseudoObsIono) {
741 usedLCs -= 1; // GIM not used
742 }
743 // max Obs
744 unsigned maxObs = obsVector.size() * usedLCs;
745
746 if (OPT->_pseudoObsIono && pseudoObsIono) {
747 maxObs -= 1; // pseudo obs iono with respect to refSat
748 }
749
750 const vector<t_pppParam*> &params = _parlist.params();
751 unsigned nPar = _parlist.nPar();
752
753 Matrix AA(maxObs, nPar); AA = 0.0;
754
755 // Real Observations
756 // -----------------
757 int iObs = -1;
758 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
759 t_pppSatObs *obs = obsVector[ii];
760 for (unsigned jj = 0; jj < usedLCs; jj++) {
761 const t_lc::type tLC = LCs[jj];
762 if (tLC == t_lc::GIM) {
763 continue;
764 }
765 ++iObs;
766 for (unsigned iPar = 0; iPar < nPar; iPar++) {
767 const t_pppParam *par = params[iPar];
768 AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
769 }
770 }
771 }
772
773 if (!(iObs+1)) {
774 continue;
775 }
776 _datumTrafo->updateIndices(sys, iObs + 1);
777
778 if (_datumTrafo->prepareAA(AA.SubMatrix(1, iObs + 1, 1, nPar), 2) != success) {
779 return failure;
780 }
781 }
782 _datumTrafo->updateNumObs();
783
784 // Datum Transformation
785 // ====================
786 if (_datumTrafo->computeTrafoMatrix() != success) {
787 return failure;
788 }
789
790 ColumnVector xFltOld = _xFlt;
791 SymmetricMatrix QFltOld = _QFlt;
792
793 _QFlt << _datumTrafo->D21() * QFltOld * _datumTrafo->D21().t();
794 _xFlt = _datumTrafo->D21() * xFltOld;
795
796#ifdef BNC_DEBUG_PPP
797 //LOG << "xFltOld:\n" << xFltOld << endl;
798 //LOG << "xFlt :\n" << _xFlt << endl;
799#endif
800
801 // Reset Ambiguities after Datum Transformation
802 // ============================================
803 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
804 char sys = usedSystems[iSys];
805 vector<t_lc::type> LCs = OPT->LCs(sys);
806 unsigned usedLCs = LCs.size();
807 if (OPT->_pseudoObsIono && !pseudoObsIono) {
808 usedLCs -= 1; // GIM not used
809 }
810 t_prn refPrnOld = refSatMapLastEpoch[sys]->prn();
811 t_prn refPrnNew = refSatMap[sys]->prn();
812 if (refPrnNew != refPrnOld) {
813 for (unsigned jj = 0; jj < usedLCs; jj++) {
814 const t_lc::type tLC = LCs[jj];
815 if (tLC == t_lc::GIM) {
816 continue;
817 }
818 resetAmb(refPrnOld, allObs, tLC);
819 }
820 }
821 }
822
823 // switch AA2 to AA1
824 // =================
825 _datumTrafo->switchAA();
826
827 _obsPool->putEpoch(_epoTime, allObs, pseudoObsIono, refSatMap);
828
829 return success;
830}
831
832// Init datum transformation
833////////////////////////////////////////////////////////////////////////////
834void t_pppFilter::initDatumTransformation(
835 const std::vector<t_pppSatObs*> &allObs, bool pseudoObsIono) {
836 unsigned trafoObs = 0;
837 const QList<char> &usedSystems = _parlist.usedSystems();
838 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
839 char sys = usedSystems[iSys];
840 int satNum = 0;
841 for (unsigned jj = 0; jj < allObs.size(); jj++) {
842 if (allObs[jj]->prn().system() == sys) {
843 satNum++;
844 }
845 }
846 // all LCs
847 unsigned realUsedLCs = OPT->LCs(sys).size();
848 // exclude pseudo obs GIM
849 if (OPT->_pseudoObsIono && !pseudoObsIono) {
850 realUsedLCs -= 1;
851 }
852
853 trafoObs += satNum * realUsedLCs;
854
855 if (OPT->_pseudoObsIono && pseudoObsIono) {
856 trafoObs -= 1; // pseudo obs iono with respect to refSat
857 }
858 }
859 _datumTrafo->setNumObs(trafoObs);
860 _datumTrafo->setNumPar(_parlist.nPar());
861 _datumTrafo->initAA();
862}
863
864//
865//////////////////////////////////////////////////////////////////////////////
866bool t_pppFilter::resetRefSatellitesLastEpoch(
867 std::vector<t_pppSatObs*> &obsVector,
868 const QMap<char, t_pppRefSat*> &refSatMap,
869 const QMap<char, t_pppRefSat*> &refSatMapLastEpoch) {
870 bool resetRefSat;
871 // reference satellite definition per system
872 const QList<char> &usedSystems = _parlist.usedSystems();
873 for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
874 resetRefSat = false;
875 char sys = usedSystems[iSys];
876 t_prn newPrn = refSatMap[sys]->prn();
877 t_prn oldPrn = refSatMapLastEpoch[sys]->prn();
878#ifdef BNC_DEBUG_PPP
879 if (oldPrn != newPrn) {
880 LOG << "oldRef: " << oldPrn.toString() << " => newRef " << newPrn.toString() << endl;
881 }
882#endif
883 vector<t_pppSatObs*>::iterator it = obsVector.begin();
884 while (it != obsVector.end()) {
885 t_pppSatObs *satObs = *it;
886 if (satObs->prn() == newPrn) {
887 resetRefSat = true;
888 satObs->setAsReference();
889 } else if (satObs->prn() == oldPrn) {
890 satObs->resetReference();
891 }
892 it++;
893 }
894 if (!resetRefSat) {
895 _obsPool->setRefSatChangeRequired(sys, true);
896 return resetRefSat;
897 }
898 }
899 return resetRefSat;
900}
Note: See TracBrowser for help on using the repository browser.