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

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