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

Last change on this file since 10013 was 10013, checked in by stuerze, 13 months ago

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