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

Last change on this file since 5859 was 5859, checked in by mervart, 10 years ago
File size: 14.1 KB
Line 
1// Part of BNC, a utility for retrieving decoding and
2// converting GNSS data streams from NTRIP broadcasters.
3//
4// Copyright (C) 2007
5// German Federal Agency for Cartography and Geodesy (BKG)
6// http://www.bkg.bund.de
7// Czech Technical University Prague, Department of Geodesy
8// http://www.fsv.cvut.cz
9//
10// Email: euref-ip@bkg.bund.de
11//
12// This program is free software; you can redistribute it and/or
13// modify it under the terms of the GNU General Public License
14// as published by the Free Software Foundation, version 2.
15//
16// This program is distributed in the hope that it will be useful,
17// but WITHOUT ANY WARRANTY; without even the implied warranty of
18// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19// GNU General Public License for more details.
20//
21// You should have received a copy of the GNU General Public License
22// along with this program; if not, write to the Free Software
23// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
24
25/* -------------------------------------------------------------------------
26 * BKG NTRIP Client
27 * -------------------------------------------------------------------------
28 *
29 * Class: t_pppFilter
30 *
31 * Purpose: Filter Adjustment
32 *
33 * Author: L. Mervart
34 *
35 * Created: 29-Jul-2014
36 *
37 * Changes:
38 *
39 * -----------------------------------------------------------------------*/
40
41#include <iostream>
42#include <iomanip>
43#include <cmath>
44#include <newmat.h>
45#include <newmatio.h>
46#include <newmatap.h>
47
48#include "pppFilter.h"
49#include "bncutils.h"
50#include "pppParlist.h"
51#include "pppObsPool.h"
52#include "pppStation.h"
53#include "pppClient.h"
54
55using namespace BNC_PPP;
56using namespace std;
57
58// Constructor
59////////////////////////////////////////////////////////////////////////////
60t_pppFilter::t_pppFilter() {
61 _parlist = 0;
62}
63
64// Destructor
65////////////////////////////////////////////////////////////////////////////
66t_pppFilter::~t_pppFilter() {
67 delete _parlist;
68}
69
70// Process Single Epoch
71////////////////////////////////////////////////////////////////////////////
72t_irc t_pppFilter::processEpoch(t_pppObsPool* obsPool) {
73
74 _numSat = 0;
75
76 if (!_parlist) {
77 _parlist = new t_pppParlist();
78 }
79
80 // Vector of all Observations
81 // --------------------------
82 t_pppObsPool::t_epoch* epoch = obsPool->lastEpoch();
83 if (!epoch) {
84 return failure;
85 }
86 vector<t_pppSatObs*>& obsVector = epoch->obsVector();
87
88 // Time of the Epoch
89 // -----------------
90 _epoTime = epoch->epoTime();
91
92 // Auxiliary vectors of processed linear combinations
93 // --------------------------------------------------
94 vector<t_lc::type> LCsCode;
95 vector<t_lc::type> LCsPhase;
96 vector<t_lc::type> LCsAll = OPT->LCs();
97 for (unsigned ii = 0; ii < LCsAll.size(); ii++) {
98 const t_lc::type& tLC = LCsAll[ii];
99 if (t_lc::includesCode(tLC) && !t_lc::includesPhase(tLC)) {
100 LCsCode.push_back(tLC);
101 }
102 else {
103 LCsPhase.push_back(tLC);
104 }
105 }
106 vector<t_lc::type> ambLCs;
107 if (LCsPhase.size() == 1) {
108 ambLCs.push_back(LCsPhase[0]);
109 }
110 else if (LCsPhase.size() > 1) {
111 ambLCs.push_back(t_lc::l1);
112 ambLCs.push_back(t_lc::l2);
113 }
114
115 // Set Parameters
116 // --------------
117 _parlist->set(_epoTime, ambLCs, obsVector);
118 const vector<t_pppParam*>& params = _parlist->params();
119
120 // Status Vector, Variance-Covariance Matrix
121 // -----------------------------------------
122 ColumnVector xFltOld = _xFlt;
123 SymmetricMatrix QFltOld = _QFlt;
124
125 _QFlt.ReSize(_parlist->nPar()); _QFlt = 0.0;
126 _xFlt.ReSize(_parlist->nPar()); _xFlt = 0.0;
127 _x0.ReSize(_parlist->nPar()); _x0 = 0.0;
128
129 for (unsigned ii = 0; ii < params.size(); ii++) {
130 const t_pppParam* par1 = params[ii];
131
132 _x0[ii] = par1->x0();
133
134 int iOld = par1->indexOld();
135 if (iOld < 0) {
136 _QFlt[ii][ii] = par1->sigma0() * par1->sigma0(); // new parameter
137 }
138 else {
139 _QFlt[ii][ii] = QFltOld[iOld][iOld] + par1->noise() * par1->noise();
140 _xFlt[ii] = xFltOld[iOld];
141 for (unsigned jj = 0; jj < ii; jj++) {
142 const t_pppParam* par2 = params[jj];
143 int jOld = par2->indexOld();
144 if (jOld >= 0) {
145 _QFlt[ii][jj] = QFltOld(iOld+1,jOld+1);
146 }
147 }
148 }
149 }
150
151 // Process LCs containing code separately
152 // --------------------------------------
153 for (unsigned ipc = 0; ipc <= 1; ipc++) {
154 const vector<t_lc::type>& LCsHlp = (ipc == 0 ? LCsCode : LCsPhase);
155 if (LCsHlp.size() > 0) {
156 if ( processLC(LCsHlp, obsVector) != success ) {
157 return failure;
158 }
159 }
160 }
161
162 _parlist->printResult(_epoTime, _QFlt, _xFlt, 0);
163
164 return success;
165}
166
167// Process Selected LCs
168////////////////////////////////////////////////////////////////////////////
169t_irc t_pppFilter::processLC(const vector<t_lc::type>& LCs,
170 vector<t_pppSatObs*>& obsVector) {
171
172 LOG.setf(ios::fixed);
173
174 // Detect Cycle Slips
175 // ------------------
176 if (detectCycleSlips(LCs, obsVector) != success) {
177 return failure;
178 }
179
180 ColumnVector xSav = _xFlt;
181 SymmetricMatrix QSav = _QFlt;
182 string epoTimeStr = string(_epoTime);
183 const vector<t_pppParam*>& params = _parlist->params();
184 unsigned maxObs = obsVector.size() * LCs.size();
185
186 // Outlier Detection Loop
187 // ----------------------
188 for (unsigned iOutlier = 0; iOutlier < maxObs; iOutlier++) {
189
190 if (iOutlier > 0) {
191 _xFlt = xSav;
192 _QFlt = QSav;
193 }
194
195 // First-Design Matrix, Terms Observed-Computed, Weight Matrix
196 // -----------------------------------------------------------
197 Matrix AA(maxObs, _parlist->nPar());
198 ColumnVector ll(maxObs);
199 UpperTriangularMatrix Sl(maxObs); Sl = 0.0;
200
201 int iObs = -1;
202 vector<t_pppSatObs*> usedObs;
203 vector<t_lc::type> usedTypes;
204 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
205 t_pppSatObs* obs = obsVector[ii];
206 if (!obs->outlier()) {
207 Matrix CC(LCs.size(), 4);
208 for (unsigned jj = 0; jj < LCs.size(); jj++) {
209 const t_lc::type tLC = LCs[jj];
210 ++iObs;
211 usedObs.push_back(obs);
212 usedTypes.push_back(tLC);
213 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
214 const t_pppParam* par = params[iPar];
215 AA[iObs][iPar] = par->partial(_epoTime, obs, tLC);
216 }
217 ll[iObs] = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA.Row(iObs+1));
218 if (LCs.size() > 1) {
219 ColumnVector coeff(4);
220 obs->lc(tLC, 0.0, 0.0, 0.0, 0.0, &coeff);
221 CC[jj][0] = coeff[0] * obs->sigma(t_lc::l1);
222 CC[jj][1] = coeff[1] * obs->sigma(t_lc::l2);
223 CC[jj][2] = coeff[2] * obs->sigma(t_lc::c1);
224 CC[jj][3] = coeff[3] * obs->sigma(t_lc::c2);
225 }
226 else {
227 Sl[iObs][iObs] = obs->sigma(tLC);
228 }
229 }
230 if (LCs.size() > 1) {
231 SymmetricMatrix QQ; QQ << CC * CC.t();
232 Sl.SymSubMatrix(iObs-LCs.size()+2, iObs+1) = Cholesky(QQ).t();
233 }
234 }
235 }
236
237 // Check number of observations, truncate matrices
238 // -----------------------------------------------
239 if (iObs+1 < OPT->_minObs) {
240 return failure;
241 }
242 AA = AA.Rows(1, iObs+1);
243 ll = ll.Rows(1, iObs+1);
244 Sl = Sl.SymSubMatrix(1, iObs+1);
245
246 // Kalman update step
247 // ------------------
248 DiagonalMatrix PP(iObs+1); PP = 0.0;
249 for (int ii = 1; ii <= iObs+1; ii++) {
250 PP(ii,ii) = 1.0 / (Sl(ii,ii) * Sl(ii,ii));
251 }
252 ColumnVector dx;
253 ColumnVector l1 = ll - AA * _xFlt;
254 kalman(AA, l1, PP, _QFlt, dx);
255 _xFlt += dx;
256
257 // Check Residuals
258 // ---------------
259 ColumnVector vv = AA * _xFlt - ll;
260 double maxOutlier = 0.0;
261 int maxOutlierIndex = -1;
262 t_lc::type maxOutlierLC = t_lc::dummy;
263 for (unsigned sysGPS = 0; sysGPS <= 1; sysGPS++) { // first GLONASS then GPS
264 for (unsigned ii = 0; ii < usedObs.size(); ii++) {
265 if (usedObs[ii]->prn().system() != 'G' || sysGPS == 1) {
266 const t_lc::type tLC = usedTypes[ii];
267 double res = fabs(vv[ii]);
268 if (res > OPT->maxRes(tLC)) {
269 if (res > fabs(maxOutlier)) {
270 maxOutlier = vv[ii];
271 maxOutlierIndex = ii;
272 maxOutlierLC = tLC;
273 }
274 }
275 }
276 }
277 if (maxOutlierIndex != -1) {
278 break;
279 }
280 }
281
282 // Mark outlier or break outlier detection loop
283 // --------------------------------------------
284 if (maxOutlierIndex > -1) {
285 t_pppSatObs* obs = usedObs[maxOutlierIndex];
286 t_pppParam* par = 0;
287 LOG << epoTimeStr << " Outlier " << t_lc::toString(maxOutlierLC) << ' '
288 << obs->prn().toString() << ' '
289 << setw(8) << setprecision(4) << maxOutlier << endl;
290 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
291 t_pppParam* hlp = params[iPar];
292 if (hlp->type() == t_pppParam::amb && hlp->prn() == obs->prn() &&
293 hlp->tLC() == usedTypes[maxOutlierIndex]) {
294 par = hlp;
295 }
296 }
297 if (par) {
298 if (par->ambResetCandidate()) {
299 resetAmb(par->prn(), obsVector, &QSav, &xSav);
300 }
301 else {
302 par->setAmbResetCandidate();
303 obs->setOutlier();
304 }
305 }
306 else {
307 obs->setOutlier();
308 }
309 }
310
311 // Print Residuals
312 // ---------------
313 else {
314 for (unsigned jj = 0; jj < LCs.size(); jj++) {
315 for (unsigned ii = 0; ii < usedObs.size(); ii++) {
316 const t_lc::type tLC = usedTypes[ii];
317 t_pppSatObs* obs = usedObs[ii];
318 if (tLC == LCs[jj]) {
319 obs->setRes(tLC, vv[ii]);
320 LOG << epoTimeStr << " RES "
321 << left << setw(3) << t_lc::toString(tLC) << right << ' '
322 << obs->prn().toString() << ' '
323 << setw(8) << setprecision(4) << vv[ii] << endl;
324 }
325 }
326 }
327 cmpDOP(LCs, AA);
328 break;
329 }
330 }
331
332 return success;
333}
334
335// Cycle-Slip Detection
336////////////////////////////////////////////////////////////////////////////
337t_irc t_pppFilter::detectCycleSlips(const vector<t_lc::type>& LCs,
338 const vector<t_pppSatObs*>& obsVector) {
339
340 const double SLIP = 20.0; // slip threshold
341 string epoTimeStr = string(_epoTime);
342 const vector<t_pppParam*>& params = _parlist->params();
343
344 for (unsigned ii = 0; ii < LCs.size(); ii++) {
345 const t_lc::type& tLC = LCs[ii];
346 if (t_lc::includesPhase(tLC)) {
347 for (unsigned iObs = 0; iObs < obsVector.size(); iObs++) {
348 const t_pppSatObs* obs = obsVector[iObs];
349
350 // Check set Slips and Jump Counters
351 // ---------------------------------
352 bool slip = false;
353
354 if (obs->slip()) {
355 LOG << "cycle slip set (obs)" << endl;;
356 slip = true;
357 }
358
359 if (_slips[obs->prn()]._obsSlipCounter != -1 &&
360 _slips[obs->prn()]._obsSlipCounter != obs->slipCounter()) {
361 LOG << "cycle slip set (obsSlipCounter)" << endl;
362 slip = true;
363 }
364 _slips[obs->prn()]._obsSlipCounter = obs->slipCounter();
365
366 if (_slips[obs->prn()]._biasJumpCounter != -1 &&
367 _slips[obs->prn()]._biasJumpCounter != obs->biasJumpCounter()) {
368 LOG << "cycle slip set (biasJumpCounter)" << endl;
369 slip = true;
370 }
371 _slips[obs->prn()]._biasJumpCounter = obs->biasJumpCounter();
372
373 // Slip Set
374 // --------
375 if (slip) {
376 resetAmb(obs->prn(), obsVector);
377 }
378
379 // Check Pre-Fit Residuals
380 // -----------------------
381 else {
382 ColumnVector AA(params.size());
383 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
384 const t_pppParam* par = params[iPar];
385 AA[iPar] = par->partial(_epoTime, obs, tLC);
386 }
387
388 double ll = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA);
389 double vv = DotProduct(AA, _xFlt) - ll;
390
391 if (fabs(vv) > SLIP) {
392 LOG << epoTimeStr << " cycle slip detected " << t_lc::toString(tLC) << ' '
393 << obs->prn().toString() << ' ' << setw(8) << setprecision(4) << vv << endl;
394 resetAmb(obs->prn(), obsVector);
395 }
396 }
397 }
398 }
399 }
400
401 return success;
402}
403
404// Reset Ambiguity Parameter (cycle slip)
405////////////////////////////////////////////////////////////////////////////
406t_irc t_pppFilter::resetAmb(t_prn prn, const vector<t_pppSatObs*>& obsVector,
407 SymmetricMatrix* QSav, ColumnVector* xSav) {
408 t_irc irc = failure;
409 vector<t_pppParam*>& params = _parlist->params();
410 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
411 t_pppParam* par = params[iPar];
412 if (par->type() == t_pppParam::amb && par->prn() == prn) {
413 int ind = par->indexNew();
414 t_lc::type tLC = par->tLC();
415 LOG << string(_epoTime) << " RESET " << par->toString() << endl;
416 delete par; par = new t_pppParam(t_pppParam::amb, prn, tLC, &obsVector);
417 par->setIndex(ind);
418 params[iPar] = par;
419 for (unsigned ii = 1; ii <= params.size(); ii++) {
420 _QFlt(ii, ind+1) = 0.0;
421 if (QSav) {
422 (*QSav)(ii, ind+1) = 0.0;
423 }
424 }
425 _QFlt(ind+1,ind+1) = par->sigma0() * par->sigma0();
426 if (QSav) {
427 (*QSav)(ind+1,ind+1) = _QFlt(ind+1,ind+1);
428 }
429 _xFlt[ind] = 0.0;
430 if (xSav) {
431 (*xSav)[ind] = _xFlt[ind];
432 }
433 _x0[ind] = par->x0();
434 irc = success;
435 }
436 }
437
438 return irc;
439}
440
441// Compute various DOP Values
442////////////////////////////////////////////////////////////////////////////
443void t_pppFilter::cmpDOP(const std::vector<t_lc::type>& LCs, const Matrix& AA) {
444
445 _dop.reset();
446 _numSat = 0;
447 try {
448 _numSat = AA.Nrows() / LCs.size();
449
450 if (_numSat < 4) {
451 return;
452 }
453
454 Matrix BB(_numSat, 4);
455
456 for (int ii = 1; ii <= _numSat; ii++) {
457 BB.Row(ii) = AA.Row(ii*LCs.size()).columns(1,4);
458 }
459
460 SymmetricMatrix NN; NN << BB.t() * BB;
461 SymmetricMatrix QQ = NN.i();
462
463 _dop.P = sqrt(QQ(1,1) + QQ(2,2) + QQ(3,3));
464 _dop.T = sqrt(QQ(4,4));
465 _dop.G = sqrt(QQ(1,1) + QQ(2,2) + QQ(3,3) + QQ(4,4));
466 }
467 catch (...) {
468 }
469}
Note: See TracBrowser for help on using the repository browser.