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

Last change on this file since 5877 was 5877, 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);
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 kalman(AA, ll, PP, _QFlt, _xFlt);
253
254 // Check Residuals
255 // ---------------
256 ColumnVector vv = AA * _xFlt - ll;
257 double maxOutlier = 0.0;
258 int maxOutlierIndex = -1;
259 t_lc::type maxOutlierLC = t_lc::dummy;
260 for (unsigned sysGPS = 0; sysGPS <= 1; sysGPS++) { // first GLONASS then GPS
261 for (unsigned ii = 0; ii < usedObs.size(); ii++) {
262 if (usedObs[ii]->prn().system() != 'G' || sysGPS == 1) {
263 const t_lc::type tLC = usedTypes[ii];
264 double res = fabs(vv[ii]);
265 if (res > OPT->maxRes(tLC)) {
266 if (res > fabs(maxOutlier)) {
267 maxOutlier = vv[ii];
268 maxOutlierIndex = ii;
269 maxOutlierLC = tLC;
270 }
271 }
272 }
273 }
274 if (maxOutlierIndex != -1) {
275 break;
276 }
277 }
278
279 // Mark outlier or break outlier detection loop
280 // --------------------------------------------
281 if (maxOutlierIndex > -1) {
282 t_pppSatObs* obs = usedObs[maxOutlierIndex];
283 t_pppParam* par = 0;
284 LOG << epoTimeStr << " Outlier " << t_lc::toString(maxOutlierLC) << ' '
285 << obs->prn().toString() << ' '
286 << setw(8) << setprecision(4) << maxOutlier << endl;
287 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
288 t_pppParam* hlp = params[iPar];
289 if (hlp->type() == t_pppParam::amb && hlp->prn() == obs->prn() &&
290 hlp->tLC() == usedTypes[maxOutlierIndex]) {
291 par = hlp;
292 }
293 }
294 if (par) {
295 if (par->ambResetCandidate()) {
296 resetAmb(par->prn(), obsVector, &QSav, &xSav);
297 }
298 else {
299 par->setAmbResetCandidate();
300 obs->setOutlier();
301 }
302 }
303 else {
304 obs->setOutlier();
305 }
306 }
307
308 // Print Residuals
309 // ---------------
310 else {
311 for (unsigned jj = 0; jj < LCs.size(); jj++) {
312 for (unsigned ii = 0; ii < usedObs.size(); ii++) {
313 const t_lc::type tLC = usedTypes[ii];
314 t_pppSatObs* obs = usedObs[ii];
315 if (tLC == LCs[jj]) {
316 obs->setRes(tLC, vv[ii]);
317 LOG << epoTimeStr << " RES "
318 << left << setw(3) << t_lc::toString(tLC) << right << ' '
319 << obs->prn().toString() << ' '
320 << setw(8) << setprecision(4) << vv[ii] << endl;
321 }
322 }
323 }
324 cmpDOP(LCs, AA);
325 break;
326 }
327 }
328
329 return success;
330}
331
332// Cycle-Slip Detection
333////////////////////////////////////////////////////////////////////////////
334t_irc t_pppFilter::detectCycleSlips(const vector<t_lc::type>& LCs,
335 const vector<t_pppSatObs*>& obsVector) {
336
337 const double SLIP = 20.0; // slip threshold
338 string epoTimeStr = string(_epoTime);
339 const vector<t_pppParam*>& params = _parlist->params();
340
341 for (unsigned ii = 0; ii < LCs.size(); ii++) {
342 const t_lc::type& tLC = LCs[ii];
343 if (t_lc::includesPhase(tLC)) {
344 for (unsigned iObs = 0; iObs < obsVector.size(); iObs++) {
345 const t_pppSatObs* obs = obsVector[iObs];
346
347 // Check set Slips and Jump Counters
348 // ---------------------------------
349 bool slip = false;
350
351 if (obs->slip()) {
352 LOG << "cycle slip set (obs)" << endl;;
353 slip = true;
354 }
355
356 if (_slips[obs->prn()]._obsSlipCounter != -1 &&
357 _slips[obs->prn()]._obsSlipCounter != obs->slipCounter()) {
358 LOG << "cycle slip set (obsSlipCounter)" << endl;
359 slip = true;
360 }
361 _slips[obs->prn()]._obsSlipCounter = obs->slipCounter();
362
363 if (_slips[obs->prn()]._biasJumpCounter != -1 &&
364 _slips[obs->prn()]._biasJumpCounter != obs->biasJumpCounter()) {
365 LOG << "cycle slip set (biasJumpCounter)" << endl;
366 slip = true;
367 }
368 _slips[obs->prn()]._biasJumpCounter = obs->biasJumpCounter();
369
370 // Slip Set
371 // --------
372 if (slip) {
373 resetAmb(obs->prn(), obsVector);
374 }
375
376 // Check Pre-Fit Residuals
377 // -----------------------
378 else {
379 ColumnVector AA(params.size());
380 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
381 const t_pppParam* par = params[iPar];
382 AA[iPar] = par->partial(_epoTime, obs, tLC);
383 }
384
385 double ll = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA);
386 double vv = DotProduct(AA, _xFlt) - ll;
387
388 if (fabs(vv) > SLIP) {
389 LOG << epoTimeStr << " cycle slip detected " << t_lc::toString(tLC) << ' '
390 << obs->prn().toString() << ' ' << setw(8) << setprecision(4) << vv << endl;
391 resetAmb(obs->prn(), obsVector);
392 }
393 }
394 }
395 }
396 }
397
398 return success;
399}
400
401// Reset Ambiguity Parameter (cycle slip)
402////////////////////////////////////////////////////////////////////////////
403t_irc t_pppFilter::resetAmb(t_prn prn, const vector<t_pppSatObs*>& obsVector,
404 SymmetricMatrix* QSav, ColumnVector* xSav) {
405 t_irc irc = failure;
406 vector<t_pppParam*>& params = _parlist->params();
407 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
408 t_pppParam* par = params[iPar];
409 if (par->type() == t_pppParam::amb && par->prn() == prn) {
410 int ind = par->indexNew();
411 t_lc::type tLC = par->tLC();
412 LOG << string(_epoTime) << " RESET " << par->toString() << endl;
413 delete par; par = new t_pppParam(t_pppParam::amb, prn, tLC, &obsVector);
414 par->setIndex(ind);
415 params[iPar] = par;
416 for (unsigned ii = 1; ii <= params.size(); ii++) {
417 _QFlt(ii, ind+1) = 0.0;
418 if (QSav) {
419 (*QSav)(ii, ind+1) = 0.0;
420 }
421 }
422 _QFlt(ind+1,ind+1) = par->sigma0() * par->sigma0();
423 if (QSav) {
424 (*QSav)(ind+1,ind+1) = _QFlt(ind+1,ind+1);
425 }
426 _xFlt[ind] = 0.0;
427 if (xSav) {
428 (*xSav)[ind] = _xFlt[ind];
429 }
430 _x0[ind] = par->x0();
431 irc = success;
432 }
433 }
434
435 return irc;
436}
437
438// Compute various DOP Values
439////////////////////////////////////////////////////////////////////////////
440void t_pppFilter::cmpDOP(const std::vector<t_lc::type>& LCs, const Matrix& AA) {
441
442 _dop.reset();
443 _numSat = 0;
444 try {
445 _numSat = AA.Nrows() / LCs.size();
446
447 if (_numSat < 4) {
448 return;
449 }
450
451 Matrix BB(_numSat, 4);
452
453 for (int ii = 1; ii <= _numSat; ii++) {
454 BB.Row(ii) = AA.Row(ii*LCs.size()).columns(1,4);
455 }
456
457 SymmetricMatrix NN; NN << BB.t() * BB;
458 SymmetricMatrix QQ = NN.i();
459
460 _dop.P = sqrt(QQ(1,1) + QQ(2,2) + QQ(3,3));
461 _dop.T = sqrt(QQ(4,4));
462 _dop.G = sqrt(QQ(1,1) + QQ(2,2) + QQ(3,3) + QQ(4,4));
463 }
464 catch (...) {
465 }
466}
Note: See TracBrowser for help on using the repository browser.