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

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