source: ntrip/trunk/BNC/src/PPP/filter.cpp@ 5790

Last change on this file since 5790 was 5748, checked in by mervart, 10 years ago
File size: 12.2 KB
RevLine 
[5743]1#include <iostream>
2#include <iomanip>
3#include <cmath>
4#include <newmat.h>
5#include <newmatio.h>
[5748]6#include <newmatap.h>
[5743]7
8#include "filter.h"
9#include "parlist.h"
10#include "obspool.h"
11#include "station.h"
[5747]12#include "pppClient.h"
[5748]13#include "bncmodel.h"
[5743]14
15using namespace BNC;
16using namespace std;
17
18// Constructor
19////////////////////////////////////////////////////////////////////////////
20t_filter::t_filter() {
21 _parlist = 0;
22}
23
24// Destructor
25////////////////////////////////////////////////////////////////////////////
26t_filter::~t_filter() {
27 delete _parlist;
28}
29
30// Process Single Epoch
31////////////////////////////////////////////////////////////////////////////
32t_irc t_filter::processEpoch(t_obsPool* obsPool) {
33
34 _numSat = 0;
35
36 if (!_parlist) {
37 _parlist = new t_parlist();
38 }
39
40 // Vector of all Observations
41 // --------------------------
42 t_obsPool::t_epoch* epoch = obsPool->lastEpoch();
43 if (!epoch) {
[5747]44 return failure;
[5743]45 }
46 vector<t_satObs*>& obsVector = epoch->obsVector();
47
48 // Time of the Epoch
49 // -----------------
50 _epoTime = epoch->epoTime();
51
52 // Auxiliary vectors of processed linear combinations
53 // --------------------------------------------------
54 vector<t_lc::type> LCsCode;
55 vector<t_lc::type> LCsPhase;
56 for (unsigned ii = 0; ii < OPT->LCs().size(); ii++) {
57 const t_lc::type& tLC = OPT->LCs()[ii];
58 if (t_lc::includesCode(tLC) && !t_lc::includesPhase(tLC)) {
59 LCsCode.push_back(tLC);
60 }
61 else {
62 LCsPhase.push_back(tLC);
63 }
64 }
65 vector<t_lc::type> ambLCs;
66 if (LCsPhase.size() == 1) {
67 ambLCs.push_back(LCsPhase[0]);
68 }
69 else if (LCsPhase.size() > 1) {
70 ambLCs.push_back(t_lc::l1);
71 ambLCs.push_back(t_lc::l2);
72 }
73
74 // Set Parameters
75 // --------------
76 _parlist->set(_epoTime, ambLCs, obsVector);
77 const vector<t_param*>& params = _parlist->params();
78
79 // Status Vector, Variance-Covariance Matrix
80 // -----------------------------------------
81 ColumnVector xFltOld = _xFlt;
82 SymmetricMatrix QFltOld = _QFlt;
83
84 _QFlt.ReSize(_parlist->nPar()); _QFlt = 0.0;
85 _xFlt.ReSize(_parlist->nPar()); _xFlt = 0.0;
86 _x0.ReSize(_parlist->nPar()); _x0 = 0.0;
87
88 for (unsigned ii = 0; ii < params.size(); ii++) {
89 const t_param* par1 = params[ii];
90
91 _x0[ii] = par1->x0();
92
93 int iOld = par1->indexOld();
94 if (iOld < 0) {
95 _QFlt[ii][ii] = par1->sigma0() * par1->sigma0(); // new parameter
96 }
97 else {
98 _QFlt[ii][ii] = QFltOld[iOld][iOld] + par1->noise() * par1->noise();
99 _xFlt[ii] = xFltOld[iOld];
100 for (unsigned jj = 0; jj < ii; jj++) {
101 const t_param* par2 = params[jj];
102 int jOld = par2->indexOld();
103 if (jOld >= 0) {
104 _QFlt[ii][jj] = QFltOld(iOld+1,jOld+1);
105 }
106 }
107 }
108 }
109
110 // Process LCs containing code separately
111 // --------------------------------------
112 for (unsigned ipc = 0; ipc <= 1; ipc++) {
113 const vector<t_lc::type>& LCsHlp = (ipc == 0 ? LCsCode : LCsPhase);
114 if (LCsHlp.size() > 0) {
[5747]115 if ( processLC(LCsHlp, obsVector) != success ) {
116 return failure;
[5743]117 }
118 }
119 }
120
[5747]121 _parlist->printResult(_epoTime, _QFlt, _xFlt, 0);
[5743]122
[5747]123 return success;
[5743]124}
125
126// Process Selected LCs
127////////////////////////////////////////////////////////////////////////////
128t_irc t_filter::processLC(const vector<t_lc::type>& LCs,
129 vector<t_satObs*>& obsVector) {
130
131 LOG.setf(ios::fixed);
132
133 // Detect Cycle Slips
134 // ------------------
[5747]135 if (detectCycleSlips(LCs, obsVector) != success) {
136 return failure;
[5743]137 }
138
139 ColumnVector xSav = _xFlt;
140 SymmetricMatrix QSav = _QFlt;
141 string epoTimeStr = string(_epoTime);
142 const vector<t_param*>& params = _parlist->params();
143 unsigned maxObs = obsVector.size() * LCs.size();
144
145 // Outlier Detection Loop
146 // ----------------------
147 for (unsigned iOutlier = 0; iOutlier < maxObs; iOutlier++) {
148
149 if (iOutlier > 0) {
150 _xFlt = xSav;
151 _QFlt = QSav;
152 }
153
154 // First-Design Matrix, Terms Observed-Computed, Weight Matrix
155 // -----------------------------------------------------------
156 Matrix AA(maxObs, _parlist->nPar());
157 ColumnVector ll(maxObs);
158 UpperTriangularMatrix Sl(maxObs); Sl = 0.0;
159
160 int iObs = -1;
161 vector<t_satObs*> usedObs;
162 vector<t_lc::type> usedTypes;
163 for (unsigned ii = 0; ii < obsVector.size(); ii++) {
164 t_satObs* obs = obsVector[ii];
165 if (!obs->outlier()) {
166 Matrix CC(LCs.size(), 4);
167 for (unsigned jj = 0; jj < LCs.size(); jj++) {
168 const t_lc::type tLC = LCs[jj];
169 ++iObs;
170 usedObs.push_back(obs);
171 usedTypes.push_back(tLC);
172 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
173 const t_param* par = params[iPar];
174 AA[iObs][iPar] = par->partial(_epoTime, obs, tLC);
175 }
176 ll[iObs] = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA.Row(iObs+1));
177 if (LCs.size() > 1) {
178 ColumnVector coeff(4);
179 obs->lc(tLC, 0.0, 0.0, 0.0, 0.0, &coeff);
180 CC[jj][0] = coeff[0] * obs->sigma(t_lc::l1);
181 CC[jj][1] = coeff[1] * obs->sigma(t_lc::l2);
182 CC[jj][2] = coeff[2] * obs->sigma(t_lc::c1);
183 CC[jj][3] = coeff[3] * obs->sigma(t_lc::c2);
184 }
185 else {
186 Sl[iObs][iObs] = obs->sigma(tLC);
187 }
188 }
189 if (LCs.size() > 1) {
190 SymmetricMatrix QQ; QQ << CC * CC.t();
191 Sl.SymSubMatrix(iObs-LCs.size()+2, iObs+1) = Cholesky(QQ).t();
192 }
193 }
194 }
195
196 // Check number of observations, truncate matrices
197 // -----------------------------------------------
[5748]198 if (iObs+1 < OPT->_minObs) {
[5747]199 return failure;
[5743]200 }
201 AA = AA.Rows(1, iObs+1);
202 ll = ll.Rows(1, iObs+1);
203 Sl = Sl.SymSubMatrix(1, iObs+1);
204
205 // Kalman update step
206 // ------------------
[5748]207 bncModel::kalman(AA, ll, Sl, _QFlt, _xFlt);
[5743]208
209 // Check Residuals
210 // ---------------
211 ColumnVector vv = AA * _xFlt - ll;
212 double maxOutlier = 0.0;
213 int maxOutlierIndex = -1;
214 t_lc::type maxOutlierLC = t_lc::dummy;
215 for (unsigned ii = 0; ii < usedObs.size(); ii++) {
216 const t_lc::type tLC = usedTypes[ii];
217 double res = fabs(vv[ii]);
[5748]218 if (res > OPT->maxRes(tLC)) {
[5743]219 if (res > fabs(maxOutlier)) {
220 maxOutlier = vv[ii];
221 maxOutlierIndex = ii;
222 maxOutlierLC = tLC;
223 }
224 }
225 }
226
227 // Mark outlier or break outlier detection loop
228 // --------------------------------------------
229 if (maxOutlierIndex > -1) {
230 t_satObs* obs = usedObs[maxOutlierIndex];
231 t_param* par = 0;
232 LOG << epoTimeStr << " Outlier " << t_lc::toString(maxOutlierLC) << ' '
233 << obs->prn().toString() << ' '
234 << setw(8) << setprecision(4) << maxOutlier << endl;
235 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
236 t_param* hlp = params[iPar];
237 if (hlp->type() == t_param::amb && hlp->prn() == obs->prn() &&
238 hlp->tLC() == usedTypes[maxOutlierIndex]) {
239 par = hlp;
240 }
241 }
242 if (par) {
243 if (par->ambResetCandidate()) {
244 resetAmb(par->prn(), obsVector, &QSav, &xSav);
245 }
246 else {
247 par->setAmbResetCandidate();
248 obs->setOutlier();
249 }
250 }
251 else {
252 obs->setOutlier();
253 }
254 }
255
256 // Print Residuals
257 // ---------------
258 else {
259 for (unsigned jj = 0; jj < LCs.size(); jj++) {
260 for (unsigned ii = 0; ii < usedObs.size(); ii++) {
261 const t_lc::type tLC = usedTypes[ii];
262 t_satObs* obs = usedObs[ii];
263 if (tLC == LCs[jj]) {
264 obs->setRes(tLC, vv[ii]);
[5748]265 LOG << epoTimeStr << " RES "
266 << left << setw(3) << t_lc::toString(tLC) << right << ' '
267 << obs->prn().toString() << ' '
268 << setw(8) << setprecision(4) << vv[ii] << endl;
[5743]269 }
270 }
271 }
272 cmpDOP(LCs, AA);
273 break;
274 }
275 }
276
[5747]277 return success;
[5743]278}
279
280// Cycle-Slip Detection
281////////////////////////////////////////////////////////////////////////////
282t_irc t_filter::detectCycleSlips(const vector<t_lc::type>& LCs,
283 const vector<t_satObs*>& obsVector) {
284
285 const double SLIP = 20.0; // slip threshold
286 string epoTimeStr = string(_epoTime);
287 const vector<t_param*>& params = _parlist->params();
288
289 for (unsigned ii = 0; ii < LCs.size(); ii++) {
290 const t_lc::type& tLC = LCs[ii];
291 if (t_lc::includesPhase(tLC)) {
292 for (unsigned iObs = 0; iObs < obsVector.size(); iObs++) {
293 const t_satObs* obs = obsVector[iObs];
294
295 // Check set Slips and Jump Counters
296 // ---------------------------------
297 bool slip = false;
298
299 if (obs->slip()) {
300 LOG << "cycle slip set (obs)" << endl;;
301 slip = true;
302 }
303
304 if (_slips[obs->prn()]._obsSlipCounter != -1 &&
305 _slips[obs->prn()]._obsSlipCounter != obs->slipCounter()) {
306 LOG << "cycle slip set (obsSlipCounter)" << endl;
307 slip = true;
308 }
309 _slips[obs->prn()]._obsSlipCounter = obs->slipCounter();
310
311 if (_slips[obs->prn()]._biasJumpCounter != -1 &&
312 _slips[obs->prn()]._biasJumpCounter != obs->biasJumpCounter()) {
313 LOG << "cycle slip set (biasJumpCounter)" << endl;
314 slip = true;
315 }
316 _slips[obs->prn()]._biasJumpCounter = obs->biasJumpCounter();
317
318 // Slip Set
319 // --------
320 if (slip) {
321 resetAmb(obs->prn(), obsVector);
322 }
323
324 // Check Pre-Fit Residuals
325 // -----------------------
326 else {
327 ColumnVector AA(params.size());
328 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
329 const t_param* par = params[iPar];
330 AA[iPar] = par->partial(_epoTime, obs, tLC);
331 }
332
333 double ll = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA);
334 double vv = DotProduct(AA, _xFlt) - ll;
335
336 if (fabs(vv) > SLIP) {
337 LOG << epoTimeStr << " cycle slip detected " << t_lc::toString(tLC) << ' '
338 << obs->prn().toString() << ' ' << setw(8) << setprecision(4) << vv << endl;
339 resetAmb(obs->prn(), obsVector);
340 }
341 }
342 }
343 }
344 }
345
[5747]346 return success;
[5743]347}
348
349// Reset Ambiguity Parameter (cycle slip)
350////////////////////////////////////////////////////////////////////////////
351t_irc t_filter::resetAmb(t_prn prn, const vector<t_satObs*>& obsVector,
352 SymmetricMatrix* QSav, ColumnVector* xSav) {
[5747]353 t_irc irc = failure;
[5743]354 vector<t_param*>& params = _parlist->params();
355 for (unsigned iPar = 0; iPar < params.size(); iPar++) {
356 t_param* par = params[iPar];
357 if (par->type() == t_param::amb && par->prn() == prn) {
358 int ind = par->indexNew();
359 t_lc::type tLC = par->tLC();
360 LOG << string(_epoTime) << " RESET " << par->toString() << endl;
361 delete par; par = new t_param(t_param::amb, prn, tLC, &obsVector);
362 par->setIndex(ind);
363 params[iPar] = par;
364 for (unsigned ii = 1; ii <= params.size(); ii++) {
365 _QFlt(ii, ind+1) = 0.0;
366 if (QSav) {
367 (*QSav)(ii, ind+1) = 0.0;
368 }
369 }
370 _QFlt(ind+1,ind+1) = par->sigma0() * par->sigma0();
371 if (QSav) {
372 (*QSav)(ind+1,ind+1) = _QFlt(ind+1,ind+1);
373 }
374 _xFlt[ind] = 0.0;
375 if (xSav) {
376 (*xSav)[ind] = _xFlt[ind];
377 }
378 _x0[ind] = par->x0();
[5747]379 irc = success;
[5743]380 }
381 }
382
383 return irc;
384}
385
386// Compute various DOP Values
387////////////////////////////////////////////////////////////////////////////
388void t_filter::cmpDOP(const std::vector<t_lc::type>& LCs, const Matrix& AA) {
389
390 _dop.reset();
391 _numSat = 0;
392 try {
393 _numSat = AA.Nrows() / LCs.size();
394
395 if (_numSat < 4) {
396 return;
397 }
398
399 Matrix BB(_numSat, 4);
400
401 for (int ii = 1; ii <= _numSat; ii++) {
402 BB.Row(ii) = AA.Row(ii*LCs.size()).columns(1,4);
403 }
404
405 SymmetricMatrix NN; NN << BB.t() * BB;
406 SymmetricMatrix QQ = NN.i();
407
408 _dop.P = sqrt(QQ(1,1) + QQ(2,2) + QQ(3,3));
409 _dop.T = sqrt(QQ(4,4));
410 _dop.G = sqrt(QQ(1,1) + QQ(2,2) + QQ(3,3) + QQ(4,4));
411 }
412 catch (...) {
413 }
414}
Note: See TracBrowser for help on using the repository browser.