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

Last change on this file since 5922 was 5922, checked in by mervart, 8 years ago
File size: 12.9 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 DiagonalMatrix PP(maxObs); PP = 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 for (unsigned jj = 0; jj < LCs.size(); jj++) {
191 const t_lc::type tLC = LCs[jj];
192 ++iObs;
193 usedObs.push_back(obs);
194 usedTypes.push_back(tLC);
195 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
196 const t_pppParam* par = params[iPar];
197 AA[iObs][iPar] = par->partial(_epoTime, obs, tLC);
198 }
199 ll[iObs] = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA.Row(iObs+1));
200 PP[iObs] = 1.0 / (obs->sigma(tLC) * obs->sigma(tLC));
201 }
202 }
203 }
204
205 // Check number of observations, truncate matrices
206 // -----------------------------------------------
207 if (iObs+1 < OPT->_minObs) {
208 return failure;
209 }
210 AA = AA.Rows(1, iObs+1);
211 ll = ll.Rows(1, iObs+1);
212 PP = PP.SymSubMatrix(1, iObs+1);
213
214 // Kalman update step
215 // ------------------
216 kalman(AA, ll, PP, _QFlt, _xFlt);
217
218 // Check Residuals
219 // ---------------
220 ColumnVector vv = AA * _xFlt - ll;
221 double maxOutlier = 0.0;
222 int maxOutlierIndex = -1;
223 t_lc::type maxOutlierLC = t_lc::dummy;
224 for (unsigned ii = 0; ii < usedObs.size(); ii++) {
225 const t_lc::type tLC = usedTypes[ii];
226 double res = fabs(vv[ii]);
227 if (res > usedObs[ii]->maxRes(tLC)) {
228 if (res > fabs(maxOutlier)) {
229 maxOutlier = vv[ii];
230 maxOutlierIndex = ii;
231 maxOutlierLC = tLC;
232 }
233 }
234 }
235
236 // Mark outlier or break outlier detection loop
237 // --------------------------------------------
238 if (maxOutlierIndex > -1) {
239 t_pppSatObs* obs = usedObs[maxOutlierIndex];
240 t_pppParam* par = 0;
241 LOG << epoTimeStr << " Outlier " << t_lc::toString(maxOutlierLC) << ' '
242 << obs->prn().toString() << ' '
243 << setw(8) << setprecision(4) << maxOutlier << endl;
244 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
245 t_pppParam* hlp = params[iPar];
246 if (hlp->type() == t_pppParam::amb && hlp->prn() == obs->prn() &&
247 hlp->tLC() == usedTypes[maxOutlierIndex]) {
248 par = hlp;
249 }
250 }
251 if (par) {
252 if (par->ambResetCandidate()) {
253 resetAmb(par->prn(), obsVector, &QSav, &xSav);
254 }
255 else {
256 par->setAmbResetCandidate();
257 obs->setOutlier();
258 }
259 }
260 else {
261 obs->setOutlier();
262 }
263 }
264
265 // Print Residuals
266 // ---------------
267 else {
268 for (unsigned jj = 0; jj < LCs.size(); jj++) {
269 for (unsigned ii = 0; ii < usedObs.size(); ii++) {
270 const t_lc::type tLC = usedTypes[ii];
271 t_pppSatObs* obs = usedObs[ii];
272 if (tLC == LCs[jj]) {
273 obs->setRes(tLC, vv[ii]);
274 LOG << epoTimeStr << " RES "
275 << left << setw(3) << t_lc::toString(tLC) << right << ' '
276 << obs->prn().toString() << ' '
277 << setw(8) << setprecision(4) << vv[ii] << endl;
278 }
279 }
280 }
281 break;
282 }
283 }
284
285 return success;
286}
287
288// Cycle-Slip Detection
289////////////////////////////////////////////////////////////////////////////
290t_irc t_pppFilter::detectCycleSlips(const vector<t_lc::type>& LCs,
291 const vector<t_pppSatObs*>& obsVector) {
292
293 const double SLIP = 20.0; // slip threshold
294 string epoTimeStr = string(_epoTime);
295 const vector<t_pppParam*>& params = _parlist->params();
296
297 for (unsigned ii = 0; ii < LCs.size(); ii++) {
298 const t_lc::type& tLC = LCs[ii];
299 if (t_lc::includesPhase(tLC)) {
300 for (unsigned iObs = 0; iObs < obsVector.size(); iObs++) {
301 const t_pppSatObs* obs = obsVector[iObs];
302
303 // Check set Slips and Jump Counters
304 // ---------------------------------
305 bool slip = false;
306
307 if (obs->slip()) {
308 LOG << "cycle slip set (obs)" << endl;;
309 slip = true;
310 }
311
312 if (_slips[obs->prn()]._obsSlipCounter != -1 &&
313 _slips[obs->prn()]._obsSlipCounter != obs->slipCounter()) {
314 LOG << "cycle slip set (obsSlipCounter)" << endl;
315 slip = true;
316 }
317 _slips[obs->prn()]._obsSlipCounter = obs->slipCounter();
318
319 if (_slips[obs->prn()]._biasJumpCounter != -1 &&
320 _slips[obs->prn()]._biasJumpCounter != obs->biasJumpCounter()) {
321 LOG << "cycle slip set (biasJumpCounter)" << endl;
322 slip = true;
323 }
324 _slips[obs->prn()]._biasJumpCounter = obs->biasJumpCounter();
325
326 // Slip Set
327 // --------
328 if (slip) {
329 resetAmb(obs->prn(), obsVector);
330 }
331
332 // Check Pre-Fit Residuals
333 // -----------------------
334 else {
335 ColumnVector AA(params.size());
336 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
337 const t_pppParam* par = params[iPar];
338 AA[iPar] = par->partial(_epoTime, obs, tLC);
339 }
340
341 double ll = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA);
342 double vv = DotProduct(AA, _xFlt) - ll;
343
344 if (fabs(vv) > SLIP) {
345 LOG << epoTimeStr << " cycle slip detected " << t_lc::toString(tLC) << ' '
346 << obs->prn().toString() << ' ' << setw(8) << setprecision(4) << vv << endl;
347 resetAmb(obs->prn(), obsVector);
348 }
349 }
350 }
351 }
352 }
353
354 return success;
355}
356
357// Reset Ambiguity Parameter (cycle slip)
358////////////////////////////////////////////////////////////////////////////
359t_irc t_pppFilter::resetAmb(t_prn prn, const vector<t_pppSatObs*>& obsVector,
360 SymmetricMatrix* QSav, ColumnVector* xSav) {
361 t_irc irc = failure;
362 vector<t_pppParam*>& params = _parlist->params();
363 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
364 t_pppParam* par = params[iPar];
365 if (par->type() == t_pppParam::amb && par->prn() == prn) {
366 int ind = par->indexNew();
367 t_lc::type tLC = par->tLC();
368 LOG << string(_epoTime) << " RESET " << par->toString() << endl;
369 delete par; par = new t_pppParam(t_pppParam::amb, prn, tLC, &obsVector);
370 par->setIndex(ind);
371 params[iPar] = par;
372 for (unsigned ii = 1; ii <= params.size(); ii++) {
373 _QFlt(ii, ind+1) = 0.0;
374 if (QSav) {
375 (*QSav)(ii, ind+1) = 0.0;
376 }
377 }
378 _QFlt(ind+1,ind+1) = par->sigma0() * par->sigma0();
379 if (QSav) {
380 (*QSav)(ind+1,ind+1) = _QFlt(ind+1,ind+1);
381 }
382 _xFlt[ind] = 0.0;
383 if (xSav) {
384 (*xSav)[ind] = _xFlt[ind];
385 }
386 _x0[ind] = par->x0();
387 irc = success;
388 }
389 }
390
391 return irc;
392}
393
394// Compute various DOP Values
395////////////////////////////////////////////////////////////////////////////
396void t_pppFilter::cmpDOP(const vector<t_pppSatObs*>& obsVector) {
397
398 _dop.reset();
399
400 try {
401 const unsigned numPar = 4;
402 Matrix AA(obsVector.size(), numPar);
403 _numSat = 0;
404 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
405 t_pppSatObs* obs = obsVector[ii];
406 if (obs->isValid() && !obs->outlier()) {
407 ++_numSat;
408 for (unsigned iPar = 0; iPar < numPar; iPar++) {
409 const t_pppParam* par = _parlist->params()[iPar];
410 AA[_numSat-1][iPar] = par->partial(_epoTime, obs, t_lc::c1);
411 }
412 }
413 }
414 if (_numSat < 4) {
415 return;
416 }
417 AA = AA.Rows(1, _numSat);
418 SymmetricMatrix NN; NN << AA.t() * AA;
419 SymmetricMatrix QQ = NN.i();
420
421 _dop.P = sqrt(QQ(1,1) + QQ(2,2) + QQ(3,3));
422 _dop.T = sqrt(QQ(4,4));
423 _dop.G = sqrt(QQ(1,1) + QQ(2,2) + QQ(3,3) + QQ(4,4));
424 }
425 catch (...) {
426 }
427}
Note: See TracBrowser for help on using the repository browser.