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

Last change on this file since 6003 was 6003, checked in by mervart, 10 years ago
File size: 13.8 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*>& allObs = epoch->obsVector();
87
88 // Time of the Epoch
89 // -----------------
90 _epoTime = epoch->epoTime();
91
92 // Set Parameters
93 // --------------
94 _parlist->set(_epoTime, allObs);
95 const vector<t_pppParam*>& params = _parlist->params();
96
97 // Status Vector, Variance-Covariance Matrix
98 // -----------------------------------------
99 ColumnVector xFltOld = _xFlt;
100 SymmetricMatrix QFltOld = _QFlt;
101
102 _QFlt.ReSize(_parlist->nPar()); _QFlt = 0.0;
103 _xFlt.ReSize(_parlist->nPar()); _xFlt = 0.0;
104 _x0.ReSize(_parlist->nPar()); _x0 = 0.0;
105
106 predictCovCrdPart(QFltOld, xFltOld);
107
108 for (unsigned ii = 3; ii < params.size(); ii++) {
109 const t_pppParam* par1 = params[ii];
110
111 _x0[ii] = par1->x0();
112
113 int iOld = par1->indexOld();
114 if (iOld < 0) {
115 _QFlt[ii][ii] = par1->sigma0() * par1->sigma0(); // new parameter
116 }
117 else {
118 _QFlt[ii][ii] = QFltOld[iOld][iOld] + par1->noise() * par1->noise();
119 _xFlt[ii] = xFltOld[iOld];
120 for (unsigned jj = 3; jj < ii; jj++) {
121 const t_pppParam* par2 = params[jj];
122 int jOld = par2->indexOld();
123 if (jOld >= 0) {
124 _QFlt[ii][jj] = QFltOld(iOld+1,jOld+1);
125 }
126 }
127 }
128 }
129
130 // Process Satellite Systems separately
131 // ------------------------------------
132 for (unsigned iSys = 0; iSys < OPT->systems().size(); iSys++) {
133 char system = OPT->systems()[iSys];
134 vector<t_pppSatObs*> obsVector;
135 for (unsigned jj = 0; jj < allObs.size(); jj++) {
136 if (allObs[jj]->prn().system() == system) {
137 obsVector.push_back(allObs[jj]);
138 }
139 }
140 if ( processSystem(OPT->LCs(system), obsVector) != success ) {
141 return failure;
142 }
143 }
144
145 cmpDOP(allObs);
146
147 _parlist->printResult(_epoTime, _QFlt, _xFlt);
148
149 return success;
150}
151
152// Process Selected LCs
153////////////////////////////////////////////////////////////////////////////
154t_irc t_pppFilter::processSystem(const vector<t_lc::type>& LCs,
155 const vector<t_pppSatObs*>& obsVector) {
156
157 LOG.setf(ios::fixed);
158
159 // Detect Cycle Slips
160 // ------------------
161 if (detectCycleSlips(LCs, obsVector) != success) {
162 return failure;
163 }
164
165 ColumnVector xSav = _xFlt;
166 SymmetricMatrix QSav = _QFlt;
167 string epoTimeStr = string(_epoTime);
168 const vector<t_pppParam*>& params = _parlist->params();
169 unsigned maxObs = obsVector.size() * LCs.size();
170
171 // Outlier Detection Loop
172 // ----------------------
173 for (unsigned iOutlier = 0; iOutlier < maxObs; iOutlier++) {
174
175 if (iOutlier > 0) {
176 _xFlt = xSav;
177 _QFlt = QSav;
178 }
179
180 // First-Design Matrix, Terms Observed-Computed, Weight Matrix
181 // -----------------------------------------------------------
182 Matrix AA(maxObs, _parlist->nPar());
183 ColumnVector ll(maxObs);
184 DiagonalMatrix PP(maxObs); PP = 0.0;
185
186 int iObs = -1;
187 vector<t_pppSatObs*> usedObs;
188 vector<t_lc::type> usedTypes;
189 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
190 t_pppSatObs* obs = obsVector[ii];
191 if (!obs->outlier()) {
192 for (unsigned jj = 0; jj < LCs.size(); jj++) {
193 const t_lc::type tLC = LCs[jj];
194 ++iObs;
195 usedObs.push_back(obs);
196 usedTypes.push_back(tLC);
197 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
198 const t_pppParam* par = params[iPar];
199 AA[iObs][iPar] = par->partial(_epoTime, obs, tLC);
200 }
201 ll[iObs] = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA.Row(iObs+1));
202 PP[iObs] = 1.0 / (obs->sigma(tLC) * obs->sigma(tLC));
203 }
204 }
205 }
206
207 // Check number of observations, truncate matrices
208 // -----------------------------------------------
209 if (iObs+1 < OPT->_minObs) {
210 return failure;
211 }
212 AA = AA.Rows(1, iObs+1);
213 ll = ll.Rows(1, iObs+1);
214 PP = PP.SymSubMatrix(1, iObs+1);
215
216 // Kalman update step
217 // ------------------
218 kalman(AA, ll, PP, _QFlt, _xFlt);
219
220 // Check Residuals
221 // ---------------
222 ColumnVector vv = AA * _xFlt - ll;
223 double maxOutlier = 0.0;
224 int maxOutlierIndex = -1;
225 t_lc::type maxOutlierLC = t_lc::dummy;
226 for (unsigned ii = 0; ii < usedObs.size(); ii++) {
227 const t_lc::type tLC = usedTypes[ii];
228 double res = fabs(vv[ii]);
229 if (res > usedObs[ii]->maxRes(tLC)) {
230 if (res > fabs(maxOutlier)) {
231 maxOutlier = vv[ii];
232 maxOutlierIndex = ii;
233 maxOutlierLC = tLC;
234 }
235 }
236 }
237
238 // Mark outlier or break outlier detection loop
239 // --------------------------------------------
240 if (maxOutlierIndex > -1) {
241 t_pppSatObs* obs = usedObs[maxOutlierIndex];
242 t_pppParam* par = 0;
243 LOG << epoTimeStr << " Outlier " << t_lc::toString(maxOutlierLC) << ' '
244 << obs->prn().toString() << ' '
245 << setw(8) << setprecision(4) << maxOutlier << endl;
246 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
247 t_pppParam* hlp = params[iPar];
248 if (hlp->type() == t_pppParam::amb && hlp->prn() == obs->prn() &&
249 hlp->tLC() == usedTypes[maxOutlierIndex]) {
250 par = hlp;
251 }
252 }
253 if (par) {
254 if (par->ambResetCandidate()) {
255 resetAmb(par->prn(), obsVector, &QSav, &xSav);
256 }
257 else {
258 par->setAmbResetCandidate();
259 obs->setOutlier();
260 }
261 }
262 else {
263 obs->setOutlier();
264 }
265 }
266
267 // Print Residuals
268 // ---------------
269 else {
270 for (unsigned jj = 0; jj < LCs.size(); jj++) {
271 for (unsigned ii = 0; ii < usedObs.size(); ii++) {
272 const t_lc::type tLC = usedTypes[ii];
273 t_pppSatObs* obs = usedObs[ii];
274 if (tLC == LCs[jj]) {
275 obs->setRes(tLC, vv[ii]);
276 LOG << epoTimeStr << " RES "
277 << left << setw(3) << t_lc::toString(tLC) << right << ' '
278 << obs->prn().toString() << ' '
279 << setw(8) << setprecision(4) << vv[ii] << endl;
280 }
281 }
282 }
283 break;
284 }
285 }
286
287 return success;
288}
289
290// Cycle-Slip Detection
291////////////////////////////////////////////////////////////////////////////
292t_irc t_pppFilter::detectCycleSlips(const vector<t_lc::type>& LCs,
293 const vector<t_pppSatObs*>& obsVector) {
294
295 const double SLIP = 20.0; // slip threshold
296 string epoTimeStr = string(_epoTime);
297 const vector<t_pppParam*>& params = _parlist->params();
298
299 for (unsigned ii = 0; ii < LCs.size(); ii++) {
300 const t_lc::type& tLC = LCs[ii];
301 if (t_lc::includesPhase(tLC)) {
302 for (unsigned iObs = 0; iObs < obsVector.size(); iObs++) {
303 const t_pppSatObs* obs = obsVector[iObs];
304
305 // Check set Slips and Jump Counters
306 // ---------------------------------
307 bool slip = false;
308
309 if (obs->slip()) {
310 LOG << "cycle slip set (obs)" << endl;;
311 slip = true;
312 }
313
314 if (_slips[obs->prn()]._obsSlipCounter != -1 &&
315 _slips[obs->prn()]._obsSlipCounter != obs->slipCounter()) {
316 LOG << "cycle slip set (obsSlipCounter)" << endl;
317 slip = true;
318 }
319 _slips[obs->prn()]._obsSlipCounter = obs->slipCounter();
320
321 if (_slips[obs->prn()]._biasJumpCounter != -1 &&
322 _slips[obs->prn()]._biasJumpCounter != obs->biasJumpCounter()) {
323 LOG << "cycle slip set (biasJumpCounter)" << endl;
324 slip = true;
325 }
326 _slips[obs->prn()]._biasJumpCounter = obs->biasJumpCounter();
327
328 // Slip Set
329 // --------
330 if (slip) {
331 resetAmb(obs->prn(), obsVector);
332 }
333
334 // Check Pre-Fit Residuals
335 // -----------------------
336 else {
337 ColumnVector AA(params.size());
338 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
339 const t_pppParam* par = params[iPar];
340 AA[iPar] = par->partial(_epoTime, obs, tLC);
341 }
342
343 double ll = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA);
344 double vv = DotProduct(AA, _xFlt) - ll;
345
346 if (fabs(vv) > SLIP) {
347 LOG << epoTimeStr << " cycle slip detected " << t_lc::toString(tLC) << ' '
348 << obs->prn().toString() << ' ' << setw(8) << setprecision(4) << vv << endl;
349 resetAmb(obs->prn(), obsVector);
350 }
351 }
352 }
353 }
354 }
355
356 return success;
357}
358
359// Reset Ambiguity Parameter (cycle slip)
360////////////////////////////////////////////////////////////////////////////
361t_irc t_pppFilter::resetAmb(t_prn prn, const vector<t_pppSatObs*>& obsVector,
362 SymmetricMatrix* QSav, ColumnVector* xSav) {
363 t_irc irc = failure;
364 vector<t_pppParam*>& params = _parlist->params();
365 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
366 t_pppParam* par = params[iPar];
367 if (par->type() == t_pppParam::amb && par->prn() == prn) {
368 int ind = par->indexNew();
369 t_lc::type tLC = par->tLC();
370 LOG << string(_epoTime) << " RESET " << par->toString() << endl;
371 delete par; par = new t_pppParam(t_pppParam::amb, prn, tLC, &obsVector);
372 par->setIndex(ind);
373 params[iPar] = par;
374 for (unsigned ii = 1; ii <= params.size(); ii++) {
375 _QFlt(ii, ind+1) = 0.0;
376 if (QSav) {
377 (*QSav)(ii, ind+1) = 0.0;
378 }
379 }
380 _QFlt(ind+1,ind+1) = par->sigma0() * par->sigma0();
381 if (QSav) {
382 (*QSav)(ind+1,ind+1) = _QFlt(ind+1,ind+1);
383 }
384 _xFlt[ind] = 0.0;
385 if (xSav) {
386 (*xSav)[ind] = _xFlt[ind];
387 }
388 _x0[ind] = par->x0();
389 irc = success;
390 }
391 }
392
393 return irc;
394}
395
396// Compute various DOP Values
397////////////////////////////////////////////////////////////////////////////
398void t_pppFilter::cmpDOP(const vector<t_pppSatObs*>& obsVector) {
399
400 _dop.reset();
401
402 try {
403 const unsigned numPar = 4;
404 Matrix AA(obsVector.size(), numPar);
405 _numSat = 0;
406 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
407 t_pppSatObs* obs = obsVector[ii];
408 if (obs->isValid() && !obs->outlier()) {
409 ++_numSat;
410 for (unsigned iPar = 0; iPar < numPar; iPar++) {
411 const t_pppParam* par = _parlist->params()[iPar];
412 AA[_numSat-1][iPar] = par->partial(_epoTime, obs, t_lc::c1);
413 }
414 }
415 }
416 if (_numSat < 4) {
417 return;
418 }
419 AA = AA.Rows(1, _numSat);
420 SymmetricMatrix NN; NN << AA.t() * AA;
421 SymmetricMatrix QQ = NN.i();
422
423 _dop.P = sqrt(QQ(1,1) + QQ(2,2) + QQ(3,3));
424 _dop.T = sqrt(QQ(4,4));
425 _dop.G = sqrt(QQ(1,1) + QQ(2,2) + QQ(3,3) + QQ(4,4));
426 }
427 catch (...) {
428 }
429}
430
431// Compute various DOP Values
432////////////////////////////////////////////////////////////////////////////
433void t_pppFilter::predictCovCrdPart(const SymmetricMatrix& QFltOld, const ColumnVector& xFltOld) {
434
435 const vector<t_pppParam*>& params = _parlist->params();
436
437 for (unsigned ii = 0; ii < 3; ii++) {
438 const t_pppParam* par1 = params[ii];
439
440 _x0[ii] = par1->x0();
441
442 int iOld = par1->indexOld();
443 if (iOld < 0) {
444 _QFlt[ii][ii] = par1->sigma0() * par1->sigma0(); // new parameter
445 }
446 else {
447 _QFlt[ii][ii] = QFltOld[iOld][iOld] + par1->noise() * par1->noise();
448 _xFlt[ii] = xFltOld[iOld];
449 for (unsigned jj = 0; jj < ii; jj++) {
450 const t_pppParam* par2 = params[jj];
451 int jOld = par2->indexOld();
452 if (jOld >= 0) {
453 _QFlt[ii][jj] = QFltOld(iOld+1,jOld+1);
454 }
455 }
456 }
457 }
458}
Note: See TracBrowser for help on using the repository browser.