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

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