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

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