source: ntrip/trunk/BNC/bncmodel.cpp@ 2111

Last change on this file since 2111 was 2111, checked in by mervart, 14 years ago

* empty log message *
File size: 14.4 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: bncParam, bncModel
30 *
31 * Purpose: Model for PPP
32 *
33 * Author: L. Mervart
34 *
35 * Created: 01-Dec-2009
36 *
37 * Changes:
38 *
39 * -----------------------------------------------------------------------*/
40
41#include <iomanip>
42#include <cmath>
43#include <newmatio.h>
44
45#include "bncmodel.h"
46#include "bncpppclient.h"
47#include "bancroft.h"
48#include "bncutils.h"
49#include "bncsettings.h"
50
51using namespace std;
52
53const unsigned MINOBS = 4;
54const double MINELE = 10.0 * M_PI / 180.0;
55const double sig_crd_0 = 100.0;
56const double sig_crd_p = 100.0;
57const double sig_clk_0 = 1000.0;
58const double sig_trp_0 = 0.01;
59const double sig_trp_p = 1e-6;
60const double sig_amb_0 = 100.0;
61const double sig_P3 = 1.0;
62const double sig_L3 = 0.01;
63
64// Constructor
65////////////////////////////////////////////////////////////////////////////
66bncParam::bncParam(bncParam::parType typeIn, int indexIn,
67 const QString& prnIn) {
68 type = typeIn;
69 index = indexIn;
70 prn = prnIn;
71 index_old = 0;
72 xx = 0.0;
73}
74
75// Destructor
76////////////////////////////////////////////////////////////////////////////
77bncParam::~bncParam() {
78}
79
80// Partial
81////////////////////////////////////////////////////////////////////////////
82double bncParam::partial(t_satData* satData, const QString& prnIn) {
83 if (type == CRD_X) {
84 return (xx - satData->xx(1)) / satData->rho;
85 }
86 else if (type == CRD_Y) {
87 return (xx - satData->xx(2)) / satData->rho;
88 }
89 else if (type == CRD_Z) {
90 return (xx - satData->xx(3)) / satData->rho;
91 }
92 else if (type == RECCLK) {
93 return 1.0;
94 }
95 else if (type == TROPO) {
96 return 1.0 / sin(satData->eleSat);
97 }
98 else if (type == AMB_L3) {
99 if (prnIn == prn) {
100 return 1.0;
101 }
102 else {
103 return 0.0;
104 }
105 }
106 return 0.0;
107}
108
109// Constructor
110////////////////////////////////////////////////////////////////////////////
111bncModel::bncModel() {
112
113 bncSettings settings;
114
115 _static = false;
116 if ( Qt::CheckState(settings.value("pppStatic").toInt()) == Qt::Checked) {
117 _static = true;
118 }
119
120 _usePhase = false;
121 if ( Qt::CheckState(settings.value("pppUsePhase").toInt()) == Qt::Checked) {
122 _usePhase = true;
123 }
124
125 _estTropo = false;
126 if ( Qt::CheckState(settings.value("pppEstTropo").toInt()) == Qt::Checked) {
127 _estTropo = true;
128 }
129
130 _xcBanc.ReSize(4); _xcBanc = 0.0;
131 _ellBanc.ReSize(3); _ellBanc = 0.0;
132
133 _params.push_back(new bncParam(bncParam::CRD_X, 1, ""));
134 _params.push_back(new bncParam(bncParam::CRD_Y, 2, ""));
135 _params.push_back(new bncParam(bncParam::CRD_Z, 3, ""));
136 _params.push_back(new bncParam(bncParam::RECCLK, 4, ""));
137 if (_estTropo) {
138 _params.push_back(new bncParam(bncParam::TROPO, 5, ""));
139 }
140
141 unsigned nPar = _params.size();
142
143 _QQ.ReSize(nPar);
144 _QQ = 0.0;
145
146 _QQ(1,1) = sig_crd_0 * sig_crd_0;
147 _QQ(2,2) = sig_crd_0 * sig_crd_0;
148 _QQ(3,3) = sig_crd_0 * sig_crd_0;
149 _QQ(4,4) = sig_clk_0 * sig_clk_0;
150 if (_estTropo) {
151 _QQ(5,5) = sig_trp_0 * sig_trp_0;
152 }
153}
154
155// Destructor
156////////////////////////////////////////////////////////////////////////////
157bncModel::~bncModel() {
158}
159
160// Bancroft Solution
161////////////////////////////////////////////////////////////////////////////
162t_irc bncModel::cmpBancroft(t_epoData* epoData) {
163
164 if (epoData->size() < MINOBS) {
165 return failure;
166 }
167
168 Matrix BB(epoData->size(), 4);
169
170 QMapIterator<QString, t_satData*> it(epoData->satData);
171 int iObs = 0;
172 while (it.hasNext()) {
173 ++iObs;
174 it.next();
175 QString prn = it.key();
176 t_satData* satData = it.value();
177 BB(iObs, 1) = satData->xx(1);
178 BB(iObs, 2) = satData->xx(2);
179 BB(iObs, 3) = satData->xx(3);
180 BB(iObs, 4) = satData->P3 + satData->clk;
181 }
182
183 bancroft(BB, _xcBanc);
184
185 // Ellipsoidal Coordinates
186 // ------------------------
187 xyz2ell(_xcBanc.data(), _ellBanc.data());
188
189 // Compute Satellite Elevations
190 // ----------------------------
191 QMutableMapIterator<QString, t_satData*> it2(epoData->satData);
192 while (it2.hasNext()) {
193 it2.next();
194 QString prn = it2.key();
195 t_satData* satData = it2.value();
196
197 ColumnVector rr = satData->xx - _xcBanc.Rows(1,3);
198 double rho = rr.norm_Frobenius();
199
200 double neu[3];
201 xyz2neu(_ellBanc.data(), rr.data(), neu);
202
203 satData->eleSat = acos( sqrt(neu[0]*neu[0] + neu[1]*neu[1]) / rho );
204 if (neu[2] < 0) {
205 satData->eleSat *= -1.0;
206 }
207 satData->azSat = atan2(neu[1], neu[0]);
208
209 if (satData->eleSat < MINELE) {
210 delete satData;
211 it2.remove();
212 }
213 }
214
215 return success;
216}
217
218// Computed Value
219////////////////////////////////////////////////////////////////////////////
220double bncModel::cmpValue(t_satData* satData) {
221
222 ColumnVector xRec(3);
223 xRec(1) = x();
224 xRec(2) = y();
225 xRec(3) = z();
226
227 double rho0 = (satData->xx - xRec).norm_Frobenius();
228 double dPhi = t_CST::omega * rho0 / t_CST::c;
229
230 xRec(1) = x() * cos(dPhi) - y() * sin(dPhi);
231 xRec(2) = y() * cos(dPhi) + x() * sin(dPhi);
232 xRec(3) = z();
233
234 satData->rho = (satData->xx - xRec).norm_Frobenius();
235
236 double tropDelay = delay_saast(satData->eleSat) +
237 trp() / sin(satData->eleSat);
238
239 return satData->rho + clk() - satData->clk + tropDelay;
240}
241
242// Tropospheric Model (Saastamoinen)
243////////////////////////////////////////////////////////////////////////////
244double bncModel::delay_saast(double Ele) {
245
246 double height = _ellBanc(3);
247
248 double pp = 1013.25 * pow(1.0 - 2.26e-5 * height, 5.225);
249 double TT = 18.0 - height * 0.0065 + 273.15;
250 double hh = 50.0 * exp(-6.396e-4 * height);
251 double ee = hh / 100.0 * exp(-37.2465 + 0.213166*TT - 0.000256908*TT*TT);
252
253 double h_km = height / 1000.0;
254
255 if (h_km < 0.0) h_km = 0.0;
256 if (h_km > 5.0) h_km = 5.0;
257 int ii = int(h_km + 1);
258 double href = ii - 1;
259
260 double bCor[6];
261 bCor[0] = 1.156;
262 bCor[1] = 1.006;
263 bCor[2] = 0.874;
264 bCor[3] = 0.757;
265 bCor[4] = 0.654;
266 bCor[5] = 0.563;
267
268 double BB = bCor[ii-1] + (bCor[ii]-bCor[ii-1]) * (h_km - href);
269
270 double zen = M_PI/2.0 - Ele;
271
272 return (0.002277/cos(zen)) * (pp + ((1255.0/TT)+0.05)*ee - BB*(tan(zen)*tan(zen)));
273}
274
275// Prediction Step of the Filter
276////////////////////////////////////////////////////////////////////////////
277void bncModel::predict(t_epoData* epoData) {
278
279 if (_usePhase) {
280
281 // Make a copy of QQ and xx, set parameter indices
282 // -----------------------------------------------
283 SymmetricMatrix QQ_old = _QQ;
284
285 for (int iPar = 1; iPar <= _params.size(); iPar++) {
286 _params[iPar-1]->index_old = _params[iPar-1]->index;
287 _params[iPar-1]->index = 0;
288 }
289
290 // Remove Ambiguity Parameters without observations
291 // ------------------------------------------------
292 int iPar = 0;
293 QMutableVectorIterator<bncParam*> it(_params);
294 while (it.hasNext()) {
295 bncParam* par = it.next();
296 bool removed = false;
297 if (par->type == bncParam::AMB_L3) {
298 if (epoData->satData.find(par->prn) == epoData->satData.end()) {
299 removed = true;
300 delete par;
301 it.remove();
302 }
303 }
304 if (! removed) {
305 ++iPar;
306 par->index = iPar;
307 }
308 }
309
310 // Add new ambiguity parameters
311 // ----------------------------
312 QMapIterator<QString, t_satData*> itObs(epoData->satData);
313 while (itObs.hasNext()) {
314 itObs.next();
315 QString prn = itObs.key();
316 bool found = false;
317 for (int iPar = 1; iPar <= _params.size(); iPar++) {
318 if (_params[iPar-1]->type == bncParam::AMB_L3 &&
319 _params[iPar-1]->prn == prn) {
320 found = true;
321 break;
322 }
323 }
324 if (!found) {
325 bncParam* par = new bncParam(bncParam::AMB_L3, _params.size()+1, prn);
326 _params.push_back(par);
327 }
328 }
329
330 int nPar = _params.size();
331 _QQ.ReSize(nPar); _QQ = 0.0;
332 for (int i1 = 1; i1 <= nPar; i1++) {
333 bncParam* p1 = _params[i1-1];
334 if (p1->index_old != 0) {
335 _QQ(p1->index, p1->index) = QQ_old(p1->index_old, p1->index_old);
336 for (int i2 = 1; i2 <= nPar; i2++) {
337 bncParam* p2 = _params[i2-1];
338 if (p2->index_old != 0) {
339 _QQ(p1->index, p2->index) = QQ_old(p1->index_old, p2->index_old);
340 }
341 }
342 }
343 }
344
345 for (int ii = 1; ii <= nPar; ii++) {
346 bncParam* par = _params[ii-1];
347 if (par->index_old == 0) {
348 _QQ(par->index, par->index) = sig_amb_0 * sig_amb_0;
349 }
350 par->index_old = par->index;
351 }
352 }
353
354 // Coordinates
355 // -----------
356 if (_static) {
357 if (x() == 0.0 && y() == 0.0 && z() == 0.0) {
358 _params[0]->xx = _xcBanc(1);
359 _params[1]->xx = _xcBanc(2);
360 _params[2]->xx = _xcBanc(3);
361 }
362 }
363 else {
364 _params[0]->xx = _xcBanc(1);
365 _params[1]->xx = _xcBanc(2);
366 _params[2]->xx = _xcBanc(3);
367
368 _QQ(1,1) += sig_crd_p * sig_crd_p;
369 _QQ(2,2) += sig_crd_p * sig_crd_p;
370 _QQ(3,3) += sig_crd_p * sig_crd_p;
371 }
372
373 // Receiver Clocks
374 // ---------------
375 _params[3]->xx = _xcBanc(4);
376 for (int iPar = 1; iPar <= _params.size(); iPar++) {
377 _QQ(iPar, 4) = 0.0;
378 }
379 _QQ(4,4) = sig_clk_0 * sig_clk_0;
380
381 // Tropospheric Delay
382 // ------------------
383 if (_estTropo) {
384 _QQ(5,5) += sig_trp_p * sig_trp_p;
385 }
386}
387
388// Update Step of the Filter (currently just a single-epoch solution)
389////////////////////////////////////////////////////////////////////////////
390t_irc bncModel::update(t_epoData* epoData) {
391
392 const static double MAXRES_CODE = 10.0;
393 const static double MAXRES_PHASE = 0.10;
394
395 ColumnVector dx;
396
397 bool outlier = false;
398
399 do {
400
401 outlier = false;
402
403 if (epoData->size() < MINOBS) {
404 return failure;
405 }
406
407 // Bancroft Solution
408 // -----------------
409 if (cmpBancroft(epoData) != success) {
410 return failure;
411 }
412
413 // Status Prediction
414 // -----------------
415 predict(epoData);
416
417 // Create First-Design Matrix
418 // --------------------------
419 unsigned nPar = _params.size();
420 unsigned nObs = _usePhase ? 2 * epoData->size() : epoData->size();
421
422 Matrix AA(nObs, nPar); // first design matrix
423 ColumnVector ll(nObs); // tems observed-computed
424 SymmetricMatrix PP(nObs); PP = 0.0;
425
426 unsigned iObs = 0;
427 QMapIterator<QString, t_satData*> itObs(epoData->satData);
428 while (itObs.hasNext()) {
429 ++iObs;
430 itObs.next();
431 QString prn = itObs.key();
432 t_satData* satData = itObs.value();
433
434 double rhoCmp = cmpValue(satData);
435
436 ll(iObs) = satData->P3 - rhoCmp;
437 PP(iObs,iObs) = 1.0 / (sig_P3 * sig_P3);
438 for (int iPar = 1; iPar <= _params.size(); iPar++) {
439 AA(iObs, iPar) = _params[iPar-1]->partial(satData, "");
440 }
441
442 if (_usePhase) {
443 ++iObs;
444 ll(iObs) = satData->L3 - rhoCmp;
445 PP(iObs,iObs) = 1.0 / (sig_L3 * sig_L3);
446 for (int iPar = 1; iPar <= _params.size(); iPar++) {
447 if (_params[iPar-1]->type == bncParam::AMB_L3 &&
448 _params[iPar-1]->prn == prn) {
449 ll(iObs) -= _params[iPar-1]->xx;
450 }
451 AA(iObs, iPar) = _params[iPar-1]->partial(satData, prn);
452 }
453 }
454 }
455
456 // Compute Kalman Update
457 // ---------------------
458 SymmetricMatrix QQsav = _QQ;
459 Matrix ATP = AA.t() * PP;
460 SymmetricMatrix NN = _QQ.i();
461 NN << NN + ATP * AA;
462 _QQ = NN.i();
463 dx = _QQ * ATP * ll;
464
465 // Outlier Detection
466 // -----------------
467 ColumnVector vv = ll - AA * dx;
468
469 double vvMaxCode = 0.0;
470 int iMaxCode = 0;
471 double vvMaxPhase = 0.0;
472 int iMaxPhase = 0;
473 if (_usePhase) {
474 for (int ii = 1; ii <= vv.Nrows(); ii += 2) {
475 if (vvMaxCode == 0.0 || fabs(vv(ii)) > vvMaxCode) {
476 vvMaxCode = fabs(vv(ii));
477 iMaxCode = ii;
478 }
479 if (vvMaxPhase == 0.0 || fabs(vv(ii+1)) > vvMaxPhase) {
480 vvMaxPhase = fabs(vv(ii+1));
481 iMaxPhase = ii;
482 }
483 }
484 }
485 else {
486 for (int ii = 1; ii <= vv.Nrows(); ii++) {
487 if (vvMaxCode == 0.0 || fabs(vv(ii)) > vvMaxCode) {
488 vvMaxCode = fabs(vv(ii));
489 iMaxCode = ii;
490 }
491 }
492 }
493
494 if (vvMaxCode > MAXRES_CODE) {
495 int iObs = 0;
496 QMutableMapIterator<QString, t_satData*> itObs(epoData->satData);
497 while (itObs.hasNext()) {
498 itObs.next();
499 iObs += 1;
500 if (iObs == iMaxCode) {
501 QString prn = itObs.key();
502 t_satData* satData = itObs.value();
503 delete satData;
504 itObs.remove();
505 _QQ = QQsav;
506 outlier = true;
507 cout << "Code " << prn.toAscii().data() << " " << vv(iObs) << endl;
508 break;
509 }
510 if (_usePhase) {
511 ++iObs;
512 }
513 }
514 }
515 else if (vvMaxPhase > MAXRES_PHASE) {
516 int iObs = 0;
517 QMutableMapIterator<QString, t_satData*> itObs(epoData->satData);
518 while (itObs.hasNext()) {
519 itObs.next();
520 iObs += 2;
521 if (iObs == iMaxPhase) {
522 QString prn = itObs.key();
523 t_satData* satData = itObs.value();
524 delete satData;
525 itObs.remove();
526 _QQ = QQsav;
527 outlier = true;
528 cout << "Code " << prn.toAscii().data() << " " << vv(iObs) << endl;
529 break;
530 }
531 }
532 }
533
534 } while (outlier);
535
536 // Set Solution Vector
537 // -------------------
538 QVectorIterator<bncParam*> itPar(_params);
539 while (itPar.hasNext()) {
540 bncParam* par = itPar.next();
541 par->xx += dx(par->index);
542 }
543
544 return success;
545}
Note: See TracBrowser for help on using the repository browser.