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

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