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

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