source: ntrip/trunk/BNC/src/PPP/filter.cpp@ 5743

Last change on this file since 5743 was 5743, checked in by mervart, 10 years ago
File size: 15.1 KB
Line 
1#include <iostream>
2#include <iomanip>
3#include <cmath>
4#include <newmat.h>
5#include <newmatio.h>
6
7#include "filter.h"
8#include "parlist.h"
9#include "ambres.h"
10#include "obspool.h"
11#include "kalman.h"
12#include "station.h"
13
14using namespace BNC;
15using namespace std;
16
17// Constructor
18////////////////////////////////////////////////////////////////////////////
19t_filter::t_filter() {
20 _parlist = 0;
21}
22
23// Destructor
24////////////////////////////////////////////////////////////////////////////
25t_filter::~t_filter() {
26 delete _parlist;
27}
28
29// Process Single Epoch
30////////////////////////////////////////////////////////////////////////////
31t_irc t_filter::processEpoch(t_obsPool* obsPool) {
32
33 _ambFixRate = 0.0;
34 _numSat = 0;
35
36 if (!_parlist) {
37 _parlist = new t_parlist();
38 }
39
40 // Vector of all Observations
41 // --------------------------
42 t_obsPool::t_epoch* epoch = obsPool->lastEpoch();
43 if (!epoch) {
44 return t_irc::failure;
45 }
46 vector<t_satObs*>& obsVector = epoch->obsVector();
47
48 // Time of the Epoch
49 // -----------------
50 _epoTime = epoch->epoTime();
51
52 // Auxiliary vectors of processed linear combinations
53 // --------------------------------------------------
54 vector<t_lc::type> LCsCode;
55 vector<t_lc::type> LCsPhase;
56 for (unsigned ii = 0; ii < OPT->LCs().size(); ii++) {
57 const t_lc::type& tLC = OPT->LCs()[ii];
58 if (t_lc::includesCode(tLC) && !t_lc::includesPhase(tLC)) {
59 LCsCode.push_back(tLC);
60 }
61 else {
62 LCsPhase.push_back(tLC);
63 }
64 }
65 vector<t_lc::type> ambLCs;
66 if (LCsPhase.size() == 1) {
67 ambLCs.push_back(LCsPhase[0]);
68 }
69 else if (LCsPhase.size() > 1) {
70 ambLCs.push_back(t_lc::l1);
71 ambLCs.push_back(t_lc::l2);
72 }
73
74 // Set Parameters
75 // --------------
76 _parlist->set(_epoTime, ambLCs, obsVector);
77 const vector<t_param*>& params = _parlist->params();
78
79 // Status Vector, Variance-Covariance Matrix
80 // -----------------------------------------
81 ColumnVector xFltOld = _xFlt;
82 SymmetricMatrix QFltOld = _QFlt;
83
84 _QFlt.ReSize(_parlist->nPar()); _QFlt = 0.0;
85 _xFlt.ReSize(_parlist->nPar()); _xFlt = 0.0;
86 _x0.ReSize(_parlist->nPar()); _x0 = 0.0;
87
88 for (unsigned ii = 0; ii < params.size(); ii++) {
89 const t_param* par1 = params[ii];
90
91 _x0[ii] = par1->x0();
92
93 int iOld = par1->indexOld();
94 if (iOld < 0) {
95 _QFlt[ii][ii] = par1->sigma0() * par1->sigma0(); // new parameter
96 }
97 else {
98 _QFlt[ii][ii] = QFltOld[iOld][iOld] + par1->noise() * par1->noise();
99 _xFlt[ii] = xFltOld[iOld];
100 for (unsigned jj = 0; jj < ii; jj++) {
101 const t_param* par2 = params[jj];
102 int jOld = par2->indexOld();
103 if (jOld >= 0) {
104 _QFlt[ii][jj] = QFltOld(iOld+1,jOld+1);
105 }
106 }
107 }
108 }
109
110 // Process LCs containing code separately
111 // --------------------------------------
112 for (unsigned ipc = 0; ipc <= 1; ipc++) {
113 const vector<t_lc::type>& LCsHlp = (ipc == 0 ? LCsCode : LCsPhase);
114 if (LCsHlp.size() > 0) {
115 if ( processLC(LCsHlp, obsVector) != t_irc::success ) {
116 return t_irc::failure;
117 }
118 }
119 }
120
121 // Copy Float Solution
122 // -------------------
123 _QFix = _QFlt;
124 _xFix = _xFlt;
125
126 _ambFixRate = 0.0;
127
128 // Constrain ZD Ambiguities
129 // ------------------------
130 if (LCsPhase.size() > 0) {
131 Matrix HH(LCsPhase.size(), params.size()); HH = 0.0;
132 ColumnVector hh(LCsPhase.size()); hh = 0.0;
133 DiagonalMatrix Sl(LCsPhase.size()); Sl = 1.e-4;
134 for (unsigned iLC = 0; iLC < LCsPhase.size(); iLC++) {
135 const t_param* ambPar = 0;
136 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
137 const t_param* par = params[iPar];
138 if (par->type() == t_param::amb && par->tLC() == LCsPhase[iLC] &&
139 par->indexOld() != -1 && !par->ambResetCandidate()) {
140 if (ambPar == 0 || ambPar->ambEleSat() < par->ambEleSat()) {
141 ambPar = par;
142 }
143 }
144 }
145 if (ambPar) {
146 HH[iLC][ambPar->indexNew()] = 1.0;
147 hh[iLC] = xFltOld[ambPar->indexOld()];
148 }
149 }
150 kalman(_QFix, _xFix, HH, hh, Sl);
151 }
152
153 // Ambiguity Resolution
154 // --------------------
155 if (OPT->ambres()) {
156 if (ambLCs.size() > 1) {
157 t_ambres::ambres(_epoTime, ambLCs, _parlist, _QFix, _xFix, true);
158 }
159 _ambFixRate = t_ambres::ambres(_epoTime, ambLCs, _parlist, _QFix, _xFix, false);
160
161 // Check/Print Fixed Residuals
162 // ---------------------------
163 bool checkResOK = true;
164 string epoTimeStr = string(_epoTime);
165 for (unsigned ii = 0; ii < OPT->LCs().size(); ii++) {
166 const t_lc::type& tLC = OPT->LCs()[ii];
167 for (unsigned iObs = 0; iObs < obsVector.size(); iObs++) {
168 const t_satObs* obs = obsVector[iObs];
169 if (obs->isValid() && !obs->outlier()) {
170 ColumnVector AA(params.size());
171 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
172 const t_param* par = params[iPar];
173 AA[iPar] = par->partial(_epoTime, obs, tLC);
174 }
175 double ll = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA);
176 double vv = DotProduct(AA, _xFix) - ll;
177
178 bool outlier = false;
179 if (fabs(vv) > (t_lc::includesCode(tLC) ? 1.5 * OPT->maxResCode() : 1.5 * OPT->maxResPhase())) {
180 outlier = true;
181 checkResOK = false;
182 }
183
184 if (OPT->logLevel() > 1) {
185 LOG << epoTimeStr << " XRES "
186 << left << setw(3) << t_lc::toString(tLC) << right << ' '
187 << obs->prn().toString() << ' '
188 << setw(8) << setprecision(4) << vv;
189 if (outlier) {
190 LOG << " outlier AR";
191 }
192 LOG << endl;
193 }
194 }
195 }
196 }
197 if (!checkResOK || _ambFixRate < OPT->ambresMinFixRate()) {
198 _ambFixRate = 0.0;
199 _xFix = _xFlt;
200 _QFix = _QFlt;
201 }
202 }
203
204 _parlist->printResult(_epoTime, _QFix, _xFix, _ambFixRate);
205
206 return t_irc::success;
207}
208
209// Process Selected LCs
210////////////////////////////////////////////////////////////////////////////
211t_irc t_filter::processLC(const vector<t_lc::type>& LCs,
212 vector<t_satObs*>& obsVector) {
213
214 LOG.setf(ios::fixed);
215
216 // Detect Cycle Slips
217 // ------------------
218 if (detectCycleSlips(LCs, obsVector) != t_irc::success) {
219 return t_irc::failure;
220 }
221
222 ColumnVector xSav = _xFlt;
223 SymmetricMatrix QSav = _QFlt;
224 string epoTimeStr = string(_epoTime);
225 const vector<t_param*>& params = _parlist->params();
226 unsigned maxObs = obsVector.size() * LCs.size();
227
228 // Outlier Detection Loop
229 // ----------------------
230 for (unsigned iOutlier = 0; iOutlier < maxObs; iOutlier++) {
231
232 if (iOutlier > 0) {
233 _xFlt = xSav;
234 _QFlt = QSav;
235 }
236
237 // First-Design Matrix, Terms Observed-Computed, Weight Matrix
238 // -----------------------------------------------------------
239 Matrix AA(maxObs, _parlist->nPar());
240 ColumnVector ll(maxObs);
241 UpperTriangularMatrix Sl(maxObs); Sl = 0.0;
242
243 int iObs = -1;
244 vector<t_satObs*> usedObs;
245 vector<t_lc::type> usedTypes;
246 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
247 t_satObs* obs = obsVector[ii];
248 if (!obs->outlier()) {
249 Matrix CC(LCs.size(), 4);
250 for (unsigned jj = 0; jj < LCs.size(); jj++) {
251 const t_lc::type tLC = LCs[jj];
252 ++iObs;
253 usedObs.push_back(obs);
254 usedTypes.push_back(tLC);
255 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
256 const t_param* par = params[iPar];
257 AA[iObs][iPar] = par->partial(_epoTime, obs, tLC);
258 }
259 ll[iObs] = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA.Row(iObs+1));
260 if (LCs.size() > 1) {
261 ColumnVector coeff(4);
262 obs->lc(tLC, 0.0, 0.0, 0.0, 0.0, &coeff);
263 CC[jj][0] = coeff[0] * obs->sigma(t_lc::l1);
264 CC[jj][1] = coeff[1] * obs->sigma(t_lc::l2);
265 CC[jj][2] = coeff[2] * obs->sigma(t_lc::c1);
266 CC[jj][3] = coeff[3] * obs->sigma(t_lc::c2);
267 }
268 else {
269 Sl[iObs][iObs] = obs->sigma(tLC);
270 }
271 }
272 if (LCs.size() > 1) {
273 SymmetricMatrix QQ; QQ << CC * CC.t();
274 Sl.SymSubMatrix(iObs-LCs.size()+2, iObs+1) = Cholesky(QQ).t();
275 }
276 }
277 }
278
279 // Check number of observations, truncate matrices
280 // -----------------------------------------------
281 if (iObs+1 < OPT->minobs()) {
282 return t_irc::failure;
283 }
284 AA = AA.Rows(1, iObs+1);
285 ll = ll.Rows(1, iObs+1);
286 Sl = Sl.SymSubMatrix(1, iObs+1);
287
288 // Kalman update step
289 // ------------------
290 kalman(_QFlt, _xFlt, AA, ll, Sl);
291
292 // Check Residuals
293 // ---------------
294 ColumnVector vv = AA * _xFlt - ll;
295 double maxOutlier = 0.0;
296 int maxOutlierIndex = -1;
297 t_lc::type maxOutlierLC = t_lc::dummy;
298 for (unsigned ii = 0; ii < usedObs.size(); ii++) {
299 const t_lc::type tLC = usedTypes[ii];
300 double res = fabs(vv[ii]);
301 if (res >
302 (t_lc::includesCode(tLC) ? OPT->maxResCode() : OPT->maxResPhase())) {
303 if (res > fabs(maxOutlier)) {
304 maxOutlier = vv[ii];
305 maxOutlierIndex = ii;
306 maxOutlierLC = tLC;
307 }
308 }
309 }
310
311 // Mark outlier or break outlier detection loop
312 // --------------------------------------------
313 if (maxOutlierIndex > -1) {
314 t_satObs* obs = usedObs[maxOutlierIndex];
315 t_param* par = 0;
316 LOG << epoTimeStr << " Outlier " << t_lc::toString(maxOutlierLC) << ' '
317 << obs->prn().toString() << ' '
318 << setw(8) << setprecision(4) << maxOutlier << endl;
319 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
320 t_param* hlp = params[iPar];
321 if (hlp->type() == t_param::amb && hlp->prn() == obs->prn() &&
322 hlp->tLC() == usedTypes[maxOutlierIndex]) {
323 par = hlp;
324 }
325 }
326 if (par) {
327 if (par->ambResetCandidate()) {
328 resetAmb(par->prn(), obsVector, &QSav, &xSav);
329 }
330 else {
331 par->setAmbResetCandidate();
332 obs->setOutlier();
333 }
334 }
335 else {
336 obs->setOutlier();
337 }
338 }
339
340 // Print Residuals
341 // ---------------
342 else {
343 for (unsigned jj = 0; jj < LCs.size(); jj++) {
344 for (unsigned ii = 0; ii < usedObs.size(); ii++) {
345 const t_lc::type tLC = usedTypes[ii];
346 t_satObs* obs = usedObs[ii];
347 if (tLC == LCs[jj]) {
348 obs->setRes(tLC, vv[ii]);
349 if (OPT->logLevel() > 1) {
350 LOG << epoTimeStr << " RES "
351 << left << setw(3) << t_lc::toString(tLC) << right << ' '
352 << obs->prn().toString() << ' '
353 << setw(8) << setprecision(4) << vv[ii] << endl;
354 }
355 }
356 }
357 }
358 cmpDOP(LCs, AA);
359 break;
360 }
361 }
362
363 return t_irc::success;
364}
365
366// Cycle-Slip Detection
367////////////////////////////////////////////////////////////////////////////
368t_irc t_filter::detectCycleSlips(const vector<t_lc::type>& LCs,
369 const vector<t_satObs*>& obsVector) {
370
371 const double SLIP = 20.0; // slip threshold
372 string epoTimeStr = string(_epoTime);
373 const vector<t_param*>& params = _parlist->params();
374
375 for (unsigned ii = 0; ii < LCs.size(); ii++) {
376 const t_lc::type& tLC = LCs[ii];
377 if (t_lc::includesPhase(tLC)) {
378 for (unsigned iObs = 0; iObs < obsVector.size(); iObs++) {
379 const t_satObs* obs = obsVector[iObs];
380
381 // Check set Slips and Jump Counters
382 // ---------------------------------
383 bool slip = false;
384
385 if (obs->slip()) {
386 LOG << "cycle slip set (obs)" << endl;;
387 slip = true;
388 }
389
390 if (_slips[obs->prn()]._obsSlipCounter != -1 &&
391 _slips[obs->prn()]._obsSlipCounter != obs->slipCounter()) {
392 LOG << "cycle slip set (obsSlipCounter)" << endl;
393 slip = true;
394 }
395 _slips[obs->prn()]._obsSlipCounter = obs->slipCounter();
396
397 if (_slips[obs->prn()]._biasJumpCounter != -1 &&
398 _slips[obs->prn()]._biasJumpCounter != obs->biasJumpCounter()) {
399 LOG << "cycle slip set (biasJumpCounter)" << endl;
400 slip = true;
401 }
402 _slips[obs->prn()]._biasJumpCounter = obs->biasJumpCounter();
403
404 // Slip Set
405 // --------
406 if (slip) {
407 resetAmb(obs->prn(), obsVector);
408 }
409
410 // Check Pre-Fit Residuals
411 // -----------------------
412 else {
413 ColumnVector AA(params.size());
414 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
415 const t_param* par = params[iPar];
416 AA[iPar] = par->partial(_epoTime, obs, tLC);
417 }
418
419 double ll = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA);
420 double vv = DotProduct(AA, _xFlt) - ll;
421
422 if (fabs(vv) > SLIP) {
423 LOG << epoTimeStr << " cycle slip detected " << t_lc::toString(tLC) << ' '
424 << obs->prn().toString() << ' ' << setw(8) << setprecision(4) << vv << endl;
425 resetAmb(obs->prn(), obsVector);
426 }
427 }
428 }
429 }
430 }
431
432 return t_irc::success;
433}
434
435// Reset Ambiguity Parameter (cycle slip)
436////////////////////////////////////////////////////////////////////////////
437t_irc t_filter::resetAmb(t_prn prn, const vector<t_satObs*>& obsVector,
438 SymmetricMatrix* QSav, ColumnVector* xSav) {
439 t_irc irc = t_irc::failure;
440 vector<t_param*>& params = _parlist->params();
441 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
442 t_param* par = params[iPar];
443 if (par->type() == t_param::amb && par->prn() == prn) {
444 int ind = par->indexNew();
445 t_lc::type tLC = par->tLC();
446 LOG << string(_epoTime) << " RESET " << par->toString() << endl;
447 delete par; par = new t_param(t_param::amb, prn, tLC, &obsVector);
448 par->setIndex(ind);
449 params[iPar] = par;
450 for (unsigned ii = 1; ii <= params.size(); ii++) {
451 _QFlt(ii, ind+1) = 0.0;
452 if (QSav) {
453 (*QSav)(ii, ind+1) = 0.0;
454 }
455 }
456 _QFlt(ind+1,ind+1) = par->sigma0() * par->sigma0();
457 if (QSav) {
458 (*QSav)(ind+1,ind+1) = _QFlt(ind+1,ind+1);
459 }
460 _xFlt[ind] = 0.0;
461 if (xSav) {
462 (*xSav)[ind] = _xFlt[ind];
463 }
464 _x0[ind] = par->x0();
465 irc = t_irc::success;
466 }
467 }
468
469 return irc;
470}
471
472// Compute various DOP Values
473////////////////////////////////////////////////////////////////////////////
474void t_filter::cmpDOP(const std::vector<t_lc::type>& LCs, const Matrix& AA) {
475
476 _dop.reset();
477 _numSat = 0;
478 try {
479 _numSat = AA.Nrows() / LCs.size();
480
481 if (_numSat < 4) {
482 return;
483 }
484
485 Matrix BB(_numSat, 4);
486
487 for (int ii = 1; ii <= _numSat; ii++) {
488 BB.Row(ii) = AA.Row(ii*LCs.size()).columns(1,4);
489 }
490
491 SymmetricMatrix NN; NN << BB.t() * BB;
492 SymmetricMatrix QQ = NN.i();
493
494 _dop.P = sqrt(QQ(1,1) + QQ(2,2) + QQ(3,3));
495 _dop.T = sqrt(QQ(4,4));
496 _dop.G = sqrt(QQ(1,1) + QQ(2,2) + QQ(3,3) + QQ(4,4));
497 }
498 catch (...) {
499 }
500}
Note: See TracBrowser for help on using the repository browser.