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

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