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

Last change on this file since 2622 was 2599, checked in by weber, 14 years ago

Output of moving average

File size: 34.8 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#include <sstream>
45
46#include "bncmodel.h"
47#include "bncapp.h"
48#include "bncpppclient.h"
49#include "bancroft.h"
50#include "bncutils.h"
51#include "bncsettings.h"
52#include "bnctides.h"
53
54using namespace std;
55
56const unsigned MINOBS = 4;
57const double MINELE_GPS = 10.0 * M_PI / 180.0;
58const double MINELE_GLO = 10.0 * M_PI / 180.0;
59const double MAXRES_CODE_GPS = 10.0;
60const double MAXRES_PHASE_GPS = 0.10;
61const double MAXRES_PHASE_GLO = 0.05;
62const double sig_crd_0 = 100.0;
63const double sig_crd_p = 100.0;
64const double sig_clk_0 = 1000.0;
65const double sig_trp_0 = 0.10;
66const double sig_trp_p = 1e-8;
67const double sig_amb_0_GPS = 1000.0;
68const double sig_amb_0_GLO = 1000.0;
69const double sig_L3_GPS = 0.02;
70const double sig_L3_GLO = 0.05;
71
72// Constructor
73////////////////////////////////////////////////////////////////////////////
74bncParam::bncParam(bncParam::parType typeIn, int indexIn,
75 const QString& prnIn) {
76 type = typeIn;
77 index = indexIn;
78 prn = prnIn;
79 index_old = 0;
80 xx = 0.0;
81
82}
83
84// Destructor
85////////////////////////////////////////////////////////////////////////////
86bncParam::~bncParam() {
87}
88
89// Partial
90////////////////////////////////////////////////////////////////////////////
91double bncParam::partial(t_satData* satData, bool phase) {
92
93 // Coordinates
94 // -----------
95 if (type == CRD_X) {
96 return (xx - satData->xx(1)) / satData->rho;
97 }
98 else if (type == CRD_Y) {
99 return (xx - satData->xx(2)) / satData->rho;
100 }
101 else if (type == CRD_Z) {
102 return (xx - satData->xx(3)) / satData->rho;
103 }
104
105 // Receiver Clocks
106 // ---------------
107 else if (type == RECCLK) {
108 return 1.0;
109 }
110
111 // Troposphere
112 // -----------
113 else if (type == TROPO) {
114 return 1.0 / sin(satData->eleSat);
115 }
116
117 // Ambiguities
118 // -----------
119 else if (type == AMB_L3) {
120 if (phase && satData->prn == prn) {
121 return 1.0;
122 }
123 else {
124 return 0.0;
125 }
126 }
127
128 // Default return
129 // --------------
130 return 0.0;
131}
132
133// Constructor
134////////////////////////////////////////////////////////////////////////////
135bncModel::bncModel(QByteArray staID) {
136
137 _staID = staID;
138
139 connect(this, SIGNAL(newMessage(QByteArray,bool)),
140 ((bncApp*)qApp), SLOT(slotMessage(const QByteArray,bool)));
141
142 bncSettings settings;
143
144 _static = false;
145 if ( Qt::CheckState(settings.value("pppStatic").toInt()) == Qt::Checked) {
146 _static = true;
147 }
148
149 _usePhase = false;
150 if ( Qt::CheckState(settings.value("pppUsePhase").toInt()) == Qt::Checked) {
151 _usePhase = true;
152 }
153
154 _estTropo = false;
155 if ( Qt::CheckState(settings.value("pppEstTropo").toInt()) == Qt::Checked) {
156 _estTropo = true;
157 }
158
159 _xcBanc.ReSize(4); _xcBanc = 0.0;
160 _ellBanc.ReSize(3); _ellBanc = 0.0;
161
162 if (_usePhase &&
163 Qt::CheckState(settings.value("pppGLONASS").toInt()) == Qt::Checked) {
164 _useGlonass = true;
165 }
166 else {
167 _useGlonass = false;
168 }
169
170 _tRangeAverage = settings.value("pppAverage").toDouble() * 60.;
171 if (_tRangeAverage < 0) {
172 _tRangeAverage = 0;
173 }
174 if (_tRangeAverage > 86400) {
175 _tRangeAverage = 86400;
176 }
177
178 int nextPar = 0;
179 _params.push_back(new bncParam(bncParam::CRD_X, ++nextPar, ""));
180 _params.push_back(new bncParam(bncParam::CRD_Y, ++nextPar, ""));
181 _params.push_back(new bncParam(bncParam::CRD_Z, ++nextPar, ""));
182 _params.push_back(new bncParam(bncParam::RECCLK, ++nextPar, ""));
183 if (_estTropo) {
184 _params.push_back(new bncParam(bncParam::TROPO, ++nextPar, ""));
185 }
186
187 unsigned nPar = _params.size();
188
189 _QQ.ReSize(nPar);
190
191 _QQ = 0.0;
192
193 for (int iPar = 1; iPar <= _params.size(); iPar++) {
194 bncParam* pp = _params[iPar-1];
195 if (pp->isCrd()) {
196 _QQ(iPar,iPar) = sig_crd_0 * sig_crd_0;
197 }
198 else if (pp->type == bncParam::RECCLK) {
199 _QQ(iPar,iPar) = sig_clk_0 * sig_clk_0;
200 }
201 else if (pp->type == bncParam::TROPO) {
202 _QQ(iPar,iPar) = sig_trp_0 * sig_trp_0;
203 }
204 }
205
206 // NMEA Output
207 // -----------
208 QString nmeaFileName = settings.value("nmeaFile").toString();
209 if (nmeaFileName.isEmpty()) {
210 _nmeaFile = 0;
211 _nmeaStream = 0;
212 }
213 else {
214 expandEnvVar(nmeaFileName);
215 _nmeaFile = new QFile(nmeaFileName);
216 if ( Qt::CheckState(settings.value("rnxAppend").toInt()) == Qt::Checked) {
217 _nmeaFile->open(QIODevice::WriteOnly | QIODevice::Append);
218 }
219 else {
220 _nmeaFile->open(QIODevice::WriteOnly);
221 }
222 _nmeaStream = new QTextStream();
223 _nmeaStream->setDevice(_nmeaFile);
224 }
225//Perlt Anfang
226 _xyzAverage[0] = 0.0; _xyzAverage[1] = 0.0; _xyzAverage[2] = 0.0;
227 _xyzAverage[3] = 0.0; _xyzAverage[4] = 0.0; _xyzAverage[5] = 0.0;
228 _xyzAverageSqr[0] = 0.0; _xyzAverageSqr[1] = 0.0; _xyzAverageSqr[2] = 0.0;
229 _xyzAverageSqr[3] = 0.0; _xyzAverageSqr[4] = 0.0; _xyzAverageSqr[5] = 0.0;
230 for (int ii = 0; ii < _posAverage.size(); ++ii) { delete _posAverage[ii]; }
231 _posAverage.clear();
232//Perlt Ende
233
234}
235
236// Destructor
237////////////////////////////////////////////////////////////////////////////
238bncModel::~bncModel() {
239 delete _nmeaStream;
240 delete _nmeaFile;
241//Perlt Anfang
242 for (int ii = 0; ii < _posAverage.size(); ++ii) { delete _posAverage[ii]; }
243//Perlt Ende
244}
245
246// Bancroft Solution
247////////////////////////////////////////////////////////////////////////////
248t_irc bncModel::cmpBancroft(t_epoData* epoData) {
249
250 if (epoData->sizeGPS() < MINOBS) {
251 _log += "bncModel::cmpBancroft: not enough data\n";
252 return failure;
253 }
254
255 Matrix BB(epoData->sizeGPS(), 4);
256
257 QMapIterator<QString, t_satData*> it(epoData->satDataGPS);
258 int iObs = 0;
259 while (it.hasNext()) {
260 ++iObs;
261 it.next();
262 QString prn = it.key();
263 t_satData* satData = it.value();
264 BB(iObs, 1) = satData->xx(1);
265 BB(iObs, 2) = satData->xx(2);
266 BB(iObs, 3) = satData->xx(3);
267 BB(iObs, 4) = satData->P3 + satData->clk;
268 }
269
270 bancroft(BB, _xcBanc);
271
272 // Ellipsoidal Coordinates
273 // ------------------------
274 xyz2ell(_xcBanc.data(), _ellBanc.data());
275
276 // Compute Satellite Elevations
277 // ----------------------------
278 QMutableMapIterator<QString, t_satData*> iGPS(epoData->satDataGPS);
279 while (iGPS.hasNext()) {
280 iGPS.next();
281 QString prn = iGPS.key();
282 t_satData* satData = iGPS.value();
283
284 ColumnVector rr = satData->xx - _xcBanc.Rows(1,3);
285 double rho = rr.norm_Frobenius();
286
287 double neu[3];
288 xyz2neu(_ellBanc.data(), rr.data(), neu);
289
290 satData->eleSat = acos( sqrt(neu[0]*neu[0] + neu[1]*neu[1]) / rho );
291 if (neu[2] < 0) {
292 satData->eleSat *= -1.0;
293 }
294 satData->azSat = atan2(neu[1], neu[0]);
295
296 if (satData->eleSat < MINELE_GPS) {
297 delete satData;
298 iGPS.remove();
299 }
300 }
301
302 QMutableMapIterator<QString, t_satData*> iGlo(epoData->satDataGlo);
303 while (iGlo.hasNext()) {
304 iGlo.next();
305 QString prn = iGlo.key();
306 t_satData* satData = iGlo.value();
307
308 ColumnVector rr = satData->xx - _xcBanc.Rows(1,3);
309 double rho = rr.norm_Frobenius();
310
311 double neu[3];
312 xyz2neu(_ellBanc.data(), rr.data(), neu);
313
314 satData->eleSat = acos( sqrt(neu[0]*neu[0] + neu[1]*neu[1]) / rho );
315 if (neu[2] < 0) {
316 satData->eleSat *= -1.0;
317 }
318 satData->azSat = atan2(neu[1], neu[0]);
319
320 if (satData->eleSat < MINELE_GLO) {
321 delete satData;
322 iGlo.remove();
323 }
324 }
325
326 return success;
327}
328
329// Computed Value
330////////////////////////////////////////////////////////////////////////////
331double bncModel::cmpValue(t_satData* satData, bool phase) {
332
333 ColumnVector xRec(3);
334 xRec(1) = x();
335 xRec(2) = y();
336 xRec(3) = z();
337
338 double rho0 = (satData->xx - xRec).norm_Frobenius();
339 double dPhi = t_CST::omega * rho0 / t_CST::c;
340
341 xRec(1) = x() * cos(dPhi) - y() * sin(dPhi);
342 xRec(2) = y() * cos(dPhi) + x() * sin(dPhi);
343 xRec(3) = z();
344
345 tides(_time, xRec);
346
347 satData->rho = (satData->xx - xRec).norm_Frobenius();
348
349 double tropDelay = delay_saast(satData->eleSat) +
350 trp() / sin(satData->eleSat);
351
352 double wind = 0.0;
353 if (phase) {
354 wind = windUp(satData->prn, satData->xx, xRec) * satData->lambda3;
355 }
356
357 return satData->rho + clk() - satData->clk + tropDelay + wind;
358}
359
360// Tropospheric Model (Saastamoinen)
361////////////////////////////////////////////////////////////////////////////
362double bncModel::delay_saast(double Ele) {
363
364 double height = _ellBanc(3);
365
366 double pp = 1013.25 * pow(1.0 - 2.26e-5 * height, 5.225);
367 double TT = 18.0 - height * 0.0065 + 273.15;
368 double hh = 50.0 * exp(-6.396e-4 * height);
369 double ee = hh / 100.0 * exp(-37.2465 + 0.213166*TT - 0.000256908*TT*TT);
370
371 double h_km = height / 1000.0;
372
373 if (h_km < 0.0) h_km = 0.0;
374 if (h_km > 5.0) h_km = 5.0;
375 int ii = int(h_km + 1);
376 double href = ii - 1;
377
378 double bCor[6];
379 bCor[0] = 1.156;
380 bCor[1] = 1.006;
381 bCor[2] = 0.874;
382 bCor[3] = 0.757;
383 bCor[4] = 0.654;
384 bCor[5] = 0.563;
385
386 double BB = bCor[ii-1] + (bCor[ii]-bCor[ii-1]) * (h_km - href);
387
388 double zen = M_PI/2.0 - Ele;
389
390 return (0.002277/cos(zen)) * (pp + ((1255.0/TT)+0.05)*ee - BB*(tan(zen)*tan(zen)));
391}
392
393// Prediction Step of the Filter
394////////////////////////////////////////////////////////////////////////////
395void bncModel::predict(t_epoData* epoData) {
396
397 bool firstCrd = x() == 0.0 && y() == 0.0 && z() == 0.0;
398
399 // Predict Parameter values, add white noise
400 // -----------------------------------------
401 for (int iPar = 1; iPar <= _params.size(); iPar++) {
402 bncParam* pp = _params[iPar-1];
403
404 // Coordinates
405 // -----------
406 if (pp->type == bncParam::CRD_X) {
407 if (firstCrd || !_static) {
408 pp->xx = _xcBanc(1);
409 }
410 _QQ(iPar,iPar) += sig_crd_p * sig_crd_p;
411 }
412 else if (pp->type == bncParam::CRD_Y) {
413 if (firstCrd || !_static) {
414 pp->xx = _xcBanc(2);
415 }
416 _QQ(iPar,iPar) += sig_crd_p * sig_crd_p;
417 }
418 else if (pp->type == bncParam::CRD_Z) {
419 if (firstCrd || !_static) {
420 pp->xx = _xcBanc(3);
421 }
422 _QQ(iPar,iPar) += sig_crd_p * sig_crd_p;
423 }
424
425 // Receiver Clocks
426 // ---------------
427 else if (pp->type == bncParam::RECCLK) {
428 pp->xx = _xcBanc(4);
429 for (int jj = 1; jj <= _params.size(); jj++) {
430 _QQ(iPar, jj) = 0.0;
431 }
432 _QQ(iPar,iPar) = sig_clk_0 * sig_clk_0;
433 }
434
435 // Tropospheric Delay
436 // ------------------
437 else if (pp->type == bncParam::TROPO) {
438 _QQ(iPar,iPar) += sig_trp_p * sig_trp_p;
439 }
440 }
441
442 // Add New Ambiguities if necessary
443 // --------------------------------
444 if (_usePhase) {
445
446 // Make a copy of QQ and xx, set parameter indices
447 // -----------------------------------------------
448 SymmetricMatrix QQ_old = _QQ;
449
450 for (int iPar = 1; iPar <= _params.size(); iPar++) {
451 _params[iPar-1]->index_old = _params[iPar-1]->index;
452 _params[iPar-1]->index = 0;
453 }
454
455 // Remove Ambiguity Parameters without observations
456 // ------------------------------------------------
457 int iPar = 0;
458 QMutableVectorIterator<bncParam*> it(_params);
459 while (it.hasNext()) {
460 bncParam* par = it.next();
461 bool removed = false;
462 if (par->type == bncParam::AMB_L3) {
463 if (epoData->satDataGPS.find(par->prn) == epoData->satDataGPS.end() &&
464 epoData->satDataGlo.find(par->prn) == epoData->satDataGlo.end() ) {
465 removed = true;
466 delete par;
467 it.remove();
468 }
469 }
470 if (! removed) {
471 ++iPar;
472 par->index = iPar;
473 }
474 }
475
476 // Add new ambiguity parameters
477 // ----------------------------
478 QMapIterator<QString, t_satData*> iGPS(epoData->satDataGPS);
479 while (iGPS.hasNext()) {
480 iGPS.next();
481 QString prn = iGPS.key();
482 t_satData* satData = iGPS.value();
483 bool found = false;
484 for (int iPar = 1; iPar <= _params.size(); iPar++) {
485 if (_params[iPar-1]->type == bncParam::AMB_L3 &&
486 _params[iPar-1]->prn == prn) {
487 found = true;
488 break;
489 }
490 }
491 if (!found) {
492 bncParam* par = new bncParam(bncParam::AMB_L3, _params.size()+1, prn);
493 _params.push_back(par);
494 par->xx = satData->L3 - cmpValue(satData, true);
495 }
496 }
497
498 QMapIterator<QString, t_satData*> iGlo(epoData->satDataGlo);
499 while (iGlo.hasNext()) {
500 iGlo.next();
501 QString prn = iGlo.key();
502 t_satData* satData = iGlo.value();
503 bool found = false;
504 for (int iPar = 1; iPar <= _params.size(); iPar++) {
505 if (_params[iPar-1]->type == bncParam::AMB_L3 &&
506 _params[iPar-1]->prn == prn) {
507 found = true;
508 break;
509 }
510 }
511 if (!found) {
512 bncParam* par = new bncParam(bncParam::AMB_L3, _params.size()+1, prn);
513 _params.push_back(par);
514 par->xx = satData->L3 - cmpValue(satData, true);
515 }
516 }
517
518 int nPar = _params.size();
519 _QQ.ReSize(nPar); _QQ = 0.0;
520 for (int i1 = 1; i1 <= nPar; i1++) {
521 bncParam* p1 = _params[i1-1];
522 if (p1->index_old != 0) {
523 _QQ(p1->index, p1->index) = QQ_old(p1->index_old, p1->index_old);
524 for (int i2 = 1; i2 <= nPar; i2++) {
525 bncParam* p2 = _params[i2-1];
526 if (p2->index_old != 0) {
527 _QQ(p1->index, p2->index) = QQ_old(p1->index_old, p2->index_old);
528 }
529 }
530 }
531 }
532
533 for (int ii = 1; ii <= nPar; ii++) {
534 bncParam* par = _params[ii-1];
535 if (par->index_old == 0) {
536 if (par->prn[0] == 'R') {
537 _QQ(par->index, par->index) = sig_amb_0_GLO * sig_amb_0_GLO;
538 }
539 else {
540 _QQ(par->index, par->index) = sig_amb_0_GPS * sig_amb_0_GPS;
541 }
542 }
543 par->index_old = par->index;
544 }
545 }
546
547}
548
549// Update Step of the Filter (currently just a single-epoch solution)
550////////////////////////////////////////////////////////////////////////////
551t_irc bncModel::update(t_epoData* epoData) {
552
553 bncSettings settings;
554 double sig_P3;
555 sig_P3 = 5.0;
556 if ( Qt::CheckState(settings.value("pppUsePhase").toInt()) == Qt::Checked ) {
557 sig_P3 = settings.value("pppSigmaCode").toDouble();
558 if (sig_P3 < 0.3 || sig_P3 > 50.0) {
559 sig_P3 = 5.0;
560 }
561 }
562
563 _log.clear();
564
565 _time = epoData->tt;
566
567 _log += "Single Point Positioning of Epoch "
568 + QByteArray(_time.timestr(1).c_str()) +
569 "\n--------------------------------------------------------------\n";
570
571 SymmetricMatrix QQsav;
572 ColumnVector dx;
573 ColumnVector vv;
574
575 // Loop over all outliers
576 // ----------------------
577 do {
578
579 // Bancroft Solution
580 // -----------------
581 if (cmpBancroft(epoData) != success) {
582 emit newMessage(_log, false);
583 return failure;
584 }
585
586 // Status Prediction
587 // -----------------
588 predict(epoData);
589
590 // Create First-Design Matrix
591 // --------------------------
592 unsigned nPar = _params.size();
593 unsigned nObs = 0;
594 if (_usePhase) {
595 nObs = 2 * epoData->sizeGPS() + epoData->sizeGlo();
596 }
597 else {
598 nObs = epoData->sizeGPS(); // Glonass pseudoranges are not used
599 }
600
601 if (nObs < nPar) {
602 _log += "bncModel::update: nObs < nPar\n";
603 emit newMessage(_log, false);
604 return failure;
605 }
606
607 Matrix AA(nObs, nPar); // first design matrix
608 ColumnVector ll(nObs); // tems observed-computed
609 DiagonalMatrix PP(nObs); PP = 0.0;
610
611 unsigned iObs = 0;
612
613 // GPS code and (optionally) phase observations
614 // --------------------------------------------
615 QMapIterator<QString, t_satData*> itGPS(epoData->satDataGPS);
616 while (itGPS.hasNext()) {
617 ++iObs;
618 itGPS.next();
619 QString prn = itGPS.key();
620 t_satData* satData = itGPS.value();
621
622 ll(iObs) = satData->P3 - cmpValue(satData, false);
623 PP(iObs,iObs) = 1.0 / (sig_P3 * sig_P3);
624 for (int iPar = 1; iPar <= _params.size(); iPar++) {
625 AA(iObs, iPar) = _params[iPar-1]->partial(satData, false);
626 }
627
628 if (_usePhase) {
629 ++iObs;
630 ll(iObs) = satData->L3 - cmpValue(satData, true);
631 PP(iObs,iObs) = 1.0 / (sig_L3_GPS * sig_L3_GPS);
632 for (int iPar = 1; iPar <= _params.size(); iPar++) {
633 if (_params[iPar-1]->type == bncParam::AMB_L3 &&
634 _params[iPar-1]->prn == prn) {
635 ll(iObs) -= _params[iPar-1]->xx;
636 }
637 AA(iObs, iPar) = _params[iPar-1]->partial(satData, true);
638 }
639 }
640 }
641
642 // Glonass phase observations
643 // --------------------------
644 if (_usePhase) {
645 QMapIterator<QString, t_satData*> itGlo(epoData->satDataGlo);
646 while (itGlo.hasNext()) {
647 ++iObs;
648 itGlo.next();
649 QString prn = itGlo.key();
650 t_satData* satData = itGlo.value();
651
652 ll(iObs) = satData->L3 - cmpValue(satData, true);
653 PP(iObs,iObs) = 1.0 / (sig_L3_GLO * sig_L3_GLO);
654 for (int iPar = 1; iPar <= _params.size(); iPar++) {
655 if (_params[iPar-1]->type == bncParam::AMB_L3 &&
656 _params[iPar-1]->prn == prn) {
657 ll(iObs) -= _params[iPar-1]->xx;
658 }
659 AA(iObs, iPar) = _params[iPar-1]->partial(satData, true);
660 }
661 }
662 }
663
664 // Compute Filter Update
665 // ---------------------
666 QQsav = _QQ;
667
668 kalman(AA, ll, PP, _QQ, dx);
669
670 vv = ll - AA * dx;
671
672 ostringstream strA;
673 strA.setf(ios::fixed);
674 ColumnVector vv_code(epoData->sizeGPS());
675 ColumnVector vv_phase(epoData->sizeGPS());
676 ColumnVector vv_glo(epoData->sizeGlo());
677
678 for (unsigned iobs = 1; iobs <= epoData->sizeGPS(); ++iobs) {
679 if (_usePhase) {
680 vv_code(iobs) = vv(2*iobs-1);
681 vv_phase(iobs) = vv(2*iobs);
682 }
683 else {
684 vv_code(iobs) = vv(iobs);
685 }
686 }
687 if (_useGlonass) {
688 for (unsigned iobs = 1; iobs <= epoData->sizeGlo(); ++iobs) {
689 vv_glo(iobs) = vv(2*epoData->sizeGPS()+iobs);
690 }
691 }
692
693 strA << "residuals code " << setw(8) << setprecision(3) << vv_code.t();
694 if (_usePhase) {
695 strA << "residuals phase " << setw(8) << setprecision(3) << vv_phase.t();
696 }
697 if (_useGlonass) {
698 strA << "residuals glo " << setw(8) << setprecision(3) << vv_glo.t();
699 }
700 _log += strA.str().c_str();
701
702 } while (outlierDetection(QQsav, vv, epoData->satDataGPS,
703 epoData->satDataGlo) != 0);
704
705 // Set Solution Vector
706 // -------------------
707 ostringstream strB;
708 strB.setf(ios::fixed);
709 QVectorIterator<bncParam*> itPar(_params);
710 while (itPar.hasNext()) {
711 bncParam* par = itPar.next();
712 par->xx += dx(par->index);
713
714 if (par->type == bncParam::RECCLK) {
715 strB << "\n clk = " << setw(6) << setprecision(3) << par->xx
716 << " +- " << setw(6) << setprecision(3)
717 << sqrt(_QQ(par->index,par->index));
718 }
719 else if (par->type == bncParam::AMB_L3) {
720 strB << "\n amb " << par->prn.toAscii().data() << " = "
721 << setw(6) << setprecision(3) << par->xx
722 << " +- " << setw(6) << setprecision(3)
723 << sqrt(_QQ(par->index,par->index));
724 }
725 else if (par->type == bncParam::TROPO) {
726 strB << "\n trp = " << par->prn.toAscii().data()
727 << setw(7) << setprecision(3) << delay_saast(M_PI/2.0) << " "
728 << setw(6) << setprecision(3) << showpos << par->xx << noshowpos
729 << " +- " << setw(6) << setprecision(3)
730 << sqrt(_QQ(par->index,par->index));
731 }
732 }
733 strB << '\n';
734 _log += strB.str().c_str();
735 emit newMessage(_log, false);
736
737
738 // Final Message (both log file and screen)
739 // ----------------------------------------
740 ostringstream strC;
741 strC.setf(ios::fixed);
742 strC << _staID.data() << " PPP "
743 << epoData->tt.timestr(1) << " " << epoData->sizeAll() << " "
744 << setw(14) << setprecision(3) << x() << " +- "
745 << setw(6) << setprecision(3) << sqrt(_QQ(1,1)) << " "
746 << setw(14) << setprecision(3) << y() << " +- "
747 << setw(6) << setprecision(3) << sqrt(_QQ(2,2)) << " "
748 << setw(14) << setprecision(3) << z() << " +- "
749 << setw(6) << setprecision(3) << sqrt(_QQ(3,3));
750
751 // NEU Output
752 // ----------
753 if (settings.value("pppOrigin").toString() == "X Y Z") {
754 double xyzRef[3];
755 double ellRef[3];
756 double _xyz[3];
757 double _neu[3];
758 xyzRef[0] = settings.value("pppRefCrdX").toDouble();
759 xyzRef[1] = settings.value("pppRefCrdY").toDouble();
760 xyzRef[2] = settings.value("pppRefCrdZ").toDouble();
761 _xyz[0] = x() - xyzRef[0];
762 _xyz[1] = y() - xyzRef[1];
763 _xyz[2] = z() - xyzRef[2];
764 xyz2ell(xyzRef, ellRef);
765 xyz2neu(ellRef, _xyz, _neu);
766 strC << " NEU "
767 << setw(8) << setprecision(3) << _neu[0] << " "
768 << setw(8) << setprecision(3) << _neu[1] << " "
769 << setw(8) << setprecision(3) << _neu[2];
770 }
771//Perlt Anfang
772// strC << endl;
773//Perlt Ende
774
775 emit newMessage(QByteArray(strC.str().c_str()), true);
776
777//Perlt Anfang
778 ostringstream strD;
779 strD.setf(ios::fixed);
780 ostringstream strE;
781 strE.setf(ios::fixed);
782
783 if (settings.value("pppOrigin").toString() != "No plot" && settings.value("pppAverage").toString() != "") {
784 double xyzRef[3];
785 if (settings.value("pppOrigin").toString() == "X Y Z") {
786 xyzRef[0] = settings.value("pppRefCrdX").toDouble();
787 xyzRef[1] = settings.value("pppRefCrdY").toDouble();
788 xyzRef[2] = settings.value("pppRefCrdZ").toDouble();
789 _xyzAverage[3]+=(x()-xyzRef[0]);
790 _xyzAverage[4]+=(y()-xyzRef[1]);
791 _xyzAverage[5]+=(z()-xyzRef[2]);
792 _xyzAverageSqr[3]+=((x()-xyzRef[0])*(x()-xyzRef[0]));
793 _xyzAverageSqr[4]+=((y()-xyzRef[1])*(y()-xyzRef[1]));
794 _xyzAverageSqr[5]+=((z()-xyzRef[2])*(z()-xyzRef[2]));
795 }
796
797 pppPos* newPos = new pppPos;
798 newPos->time = epoData->tt;
799 newPos->xyz[0] = x();
800 newPos->xyz[1] = y();
801 newPos->xyz[2] = z();
802 _posAverage.push_back(newPos);
803
804 _xyzAverage[0]+=x();
805 _xyzAverage[1]+=y();
806 _xyzAverage[2]+=z();
807 _xyzAverageSqr[0]+=(x()*x());
808 _xyzAverageSqr[1]+=(y()*y());
809 _xyzAverageSqr[2]+=(z()*z());
810
811 QMutableVectorIterator<pppPos*> it(_posAverage);
812 while (it.hasNext()) {
813 pppPos* pp = it.next();
814 if ( (epoData->tt - pp->time) >= _tRangeAverage ) {
815 _xyzAverage[0]-=pp->xyz[0];
816 _xyzAverage[1]-=pp->xyz[1];
817 _xyzAverage[2]-=pp->xyz[2];
818 _xyzAverageSqr[0]-=(pp->xyz[0]*pp->xyz[0]);
819 _xyzAverageSqr[1]-=(pp->xyz[1]*pp->xyz[1]);
820 _xyzAverageSqr[2]-=(pp->xyz[2]*pp->xyz[2]);
821 _xyzAverage[3]-=(pp->xyz[0]-xyzRef[0]);
822 _xyzAverage[4]-=(pp->xyz[1]-xyzRef[1]);
823 _xyzAverage[5]-=(pp->xyz[2]-xyzRef[2]);
824 _xyzAverageSqr[3]-=((pp->xyz[0]-xyzRef[0])*(pp->xyz[0]-xyzRef[0]));
825 _xyzAverageSqr[4]-=((pp->xyz[1]-xyzRef[1])*(pp->xyz[1]-xyzRef[1]));
826 _xyzAverageSqr[5]-=((pp->xyz[2]-xyzRef[2])*(pp->xyz[2]-xyzRef[2]));
827 delete pp;
828 it.remove();
829 }
830 }
831 _xyzAverageN=_posAverage.size();
832 double AveX;
833 double AveY;
834 double AveZ;
835 double dAveX;
836 double dAveY;
837 double dAveZ;
838 if (_xyzAverageN>1) {
839 AveX= _xyzAverage[0]/_xyzAverageN;
840 AveY= _xyzAverage[1]/_xyzAverageN;
841 AveZ= _xyzAverage[2]/_xyzAverageN;
842 dAveX= sqrt((_xyzAverageSqr[0]-_xyzAverage[0]*_xyzAverage[0]/(_xyzAverageN))/(_xyzAverageN-1));
843 dAveY= sqrt((_xyzAverageSqr[1]-_xyzAverage[1]*_xyzAverage[1]/(_xyzAverageN))/(_xyzAverageN-1));
844 dAveZ= sqrt((_xyzAverageSqr[2]-_xyzAverage[2]*_xyzAverage[2]/(_xyzAverageN))/(_xyzAverageN-1));
845 strD << _staID.data() << " AVE-XYZ "
846 << epoData->tt.timestr(1) << " "
847 << setw(13) << setprecision(3) << AveX << " +- "
848 << setw(6) << setprecision(3) << dAveX << " "
849 << setw(14) << setprecision(3) << AveY << " +- "
850 << setw(6) << setprecision(3) << dAveY << " "
851 << setw(14) << setprecision(3) << AveZ << " +- "
852 << setw(6) << setprecision(3) << dAveZ;
853 emit newMessage(QByteArray(strD.str().c_str()), true);
854 }
855 if (settings.value("pppOrigin").toString() == "X Y Z" && settings.value("pppAverage").toString() != "") {
856 double _xyz[3];
857 double ellRef[3];
858 double _dxyz[3];
859 double _neu[3];
860 double _dneu[3];
861 xyz2ell(xyzRef, ellRef);
862 _xyz[0]= _xyzAverage[3]/_xyzAverageN;
863 _xyz[1]= _xyzAverage[4]/_xyzAverageN;
864 _xyz[2]= _xyzAverage[5]/_xyzAverageN;
865 if (_xyzAverageN>1) {
866 _dxyz[0]= sqrt((_xyzAverageSqr[3]-_xyzAverage[3]*_xyzAverage[3]/(_xyzAverageN))/(_xyzAverageN-1));
867 _dxyz[1]= sqrt((_xyzAverageSqr[4]-_xyzAverage[4]*_xyzAverage[4]/(_xyzAverageN))/(_xyzAverageN-1));
868 _dxyz[2]= sqrt((_xyzAverageSqr[5]-_xyzAverage[5]*_xyzAverage[5]/(_xyzAverageN))/(_xyzAverageN-1));
869 xyz2neu(ellRef, _xyz, _neu);
870 xyz2neu(ellRef, _dxyz, _dneu);
871 _dneu[0]=sqrt(_dneu[0]*_dneu[0]);
872 _dneu[1]=sqrt(_dneu[1]*_dneu[1]);
873 _dneu[2]=sqrt(_dneu[2]*_dneu[2]);
874 strE << _staID.data() << " AVE-NEU "
875 << epoData->tt.timestr(1) << " "
876 << setw(8) << setprecision(3) << _neu[0] << " +- "
877 << setw(6) << setprecision(3) << _dneu[0] << " "
878 << setw(8) << setprecision(3) << _neu[1] << " +- "
879 << setw(6) << setprecision(3) << _dneu[1] << " "
880 << setw(8) << setprecision(3) << _neu[2] << " +- "
881 << setw(6) << setprecision(3) << _dneu[2];
882 emit newMessage(QByteArray(strE.str().c_str()), true);
883 }
884 }
885 }
886//Perlt Ende
887
888
889 // NMEA Output
890 // -----------
891 double xyz[3];
892 xyz[0] = x();
893 xyz[1] = y();
894 xyz[2] = z();
895 double ell[3];
896 xyz2ell(xyz, ell);
897 double phiDeg = ell[0] * 180 / M_PI;
898 double lamDeg = ell[1] * 180 / M_PI;
899
900 char phiCh = 'N';
901 if (phiDeg < 0) {
902 phiDeg = -phiDeg;
903 phiCh = 'S';
904 }
905 char lamCh = 'E';
906 if (lamDeg < 0) {
907 lamDeg = -lamDeg;
908 lamCh = 'W';
909 }
910
911 string datestr = epoData->tt.datestr(0); // yyyymmdd
912 ostringstream strRMC;
913 strRMC.setf(ios::fixed);
914 strRMC << "GPRMC,"
915 << epoData->tt.timestr(0,0) << ",A,"
916 << setw(2) << setfill('0') << int(phiDeg)
917 << setw(6) << setprecision(3) << setfill('0')
918 << fmod(60*phiDeg,60) << ',' << phiCh << ','
919 << setw(3) << setfill('0') << int(lamDeg)
920 << setw(6) << setprecision(3) << setfill('0')
921 << fmod(60*lamDeg,60) << ',' << lamCh << ",,,"
922 << datestr[6] << datestr[7] << datestr[4] << datestr[5]
923 << datestr[2] << datestr[3] << ",,";
924
925 writeNMEAstr(QString(strRMC.str().c_str()));
926
927 double dop = 2.0; // TODO
928
929 ostringstream strGGA;
930 strGGA.setf(ios::fixed);
931 strGGA << "GPGGA,"
932 << epoData->tt.timestr(0,0) << ','
933 << setw(2) << setfill('0') << int(phiDeg)
934 << setw(10) << setprecision(7) << setfill('0')
935 << fmod(60*phiDeg,60) << ',' << phiCh << ','
936 << setw(3) << setfill('0') << int(lamDeg)
937 << setw(10) << setprecision(7) << setfill('0')
938 << fmod(60*lamDeg,60) << ',' << lamCh
939 << ",1," << setw(2) << setfill('0') << epoData->sizeAll() << ','
940 << setw(3) << setprecision(1) << dop << ','
941 << setprecision(3) << ell[2] << ",M,0.0,M,,";
942
943 writeNMEAstr(QString(strGGA.str().c_str()));
944
945 return success;
946}
947
948// Outlier Detection
949////////////////////////////////////////////////////////////////////////////
950int bncModel::outlierDetection(const SymmetricMatrix& QQsav,
951 const ColumnVector& vv,
952 QMap<QString, t_satData*>& satDataGPS,
953 QMap<QString, t_satData*>& satDataGlo) {
954
955 double vvMaxCodeGPS = 0.0;
956 double vvMaxPhaseGPS = 0.0;
957 double vvMaxPhaseGlo = 0.0;
958 QMutableMapIterator<QString, t_satData*> itMaxCodeGPS(satDataGPS);
959 QMutableMapIterator<QString, t_satData*> itMaxPhaseGPS(satDataGPS);
960 QMutableMapIterator<QString, t_satData*> itMaxPhaseGlo(satDataGlo);
961
962 int ii = 0;
963
964 // GPS code and (optionally) phase residuals
965 // -----------------------------------------
966 QMutableMapIterator<QString, t_satData*> itGPS(satDataGPS);
967 while (itGPS.hasNext()) {
968 itGPS.next();
969 ++ii;
970
971 if (vvMaxCodeGPS == 0.0 || fabs(vv(ii)) > vvMaxCodeGPS) {
972 vvMaxCodeGPS = fabs(vv(ii));
973 itMaxCodeGPS = itGPS;
974 }
975
976 if (_usePhase) {
977 ++ii;
978 if (vvMaxPhaseGPS == 0.0 || fabs(vv(ii)) > vvMaxPhaseGPS) {
979 vvMaxPhaseGPS = fabs(vv(ii));
980 itMaxPhaseGPS = itGPS;
981 }
982 }
983 }
984
985 // Glonass phase residuals
986 // -----------------------
987 if (_usePhase) {
988 QMutableMapIterator<QString, t_satData*> itGlo(satDataGlo);
989 while (itGlo.hasNext()) {
990 itGlo.next();
991 ++ii;
992 if (vvMaxPhaseGlo == 0.0 || fabs(vv(ii)) > vvMaxPhaseGlo) {
993 vvMaxPhaseGlo = fabs(vv(ii));
994 itMaxPhaseGlo = itGlo;
995 }
996 }
997 }
998
999 if (vvMaxPhaseGlo > MAXRES_PHASE_GLO) {
1000 QString prn = itMaxPhaseGlo.key();
1001 t_satData* satData = itMaxPhaseGlo.value();
1002 delete satData;
1003 itMaxPhaseGlo.remove();
1004 _QQ = QQsav;
1005
1006 _log += "Outlier Phase " + prn.toAscii() + " "
1007 + QByteArray::number(vvMaxPhaseGlo, 'f', 3) + "\n";
1008
1009 return 1;
1010 }
1011
1012 else if (vvMaxCodeGPS > MAXRES_CODE_GPS) {
1013 QString prn = itMaxCodeGPS.key();
1014 t_satData* satData = itMaxCodeGPS.value();
1015 delete satData;
1016 itMaxCodeGPS.remove();
1017 _QQ = QQsav;
1018
1019 _log += "Outlier Code " + prn.toAscii() + " "
1020 + QByteArray::number(vvMaxCodeGPS, 'f', 3) + "\n";
1021
1022 return 1;
1023 }
1024 else if (vvMaxPhaseGPS > MAXRES_PHASE_GPS) {
1025 QString prn = itMaxPhaseGPS.key();
1026 t_satData* satData = itMaxPhaseGPS.value();
1027 delete satData;
1028 itMaxPhaseGPS.remove();
1029 _QQ = QQsav;
1030
1031 _log += "Outlier Phase " + prn.toAscii() + " "
1032 + QByteArray::number(vvMaxPhaseGPS, 'f', 3) + "\n";
1033
1034 return 1;
1035 }
1036
1037 return 0;
1038}
1039
1040//
1041////////////////////////////////////////////////////////////////////////////
1042void bncModel::writeNMEAstr(const QString& nmStr) {
1043
1044 unsigned char XOR = 0;
1045 for (int ii = 0; ii < nmStr.length(); ii++) {
1046 XOR ^= (unsigned char) nmStr[ii].toAscii();
1047 }
1048
1049 QString outStr = '$' + nmStr
1050 + QString("*%1\n").arg(int(XOR), 0, 16).toUpper();
1051
1052 if (_nmeaStream) {
1053 *_nmeaStream << outStr;
1054 _nmeaStream->flush();
1055 }
1056
1057 emit newNMEAstr(outStr.toAscii());
1058}
1059
1060
1061////
1062//////////////////////////////////////////////////////////////////////////////
1063void bncModel::kalman(const Matrix& AA, const ColumnVector& ll,
1064 const DiagonalMatrix& PP,
1065 SymmetricMatrix& QQ, ColumnVector& dx) {
1066
1067 int nObs = AA.Nrows();
1068 int nPar = AA.Ncols();
1069
1070 UpperTriangularMatrix SS = Cholesky(QQ).t();
1071
1072 Matrix SA = SS*AA.t();
1073 Matrix SRF(nObs+nPar, nObs+nPar); SRF = 0;
1074 for (int ii = 1; ii <= nObs; ++ii) {
1075 SRF(ii,ii) = 1.0 / sqrt(PP(ii,ii));
1076 }
1077
1078 SRF.SubMatrix (nObs+1, nObs+nPar, 1, nObs) = SA;
1079 SRF.SymSubMatrix(nObs+1, nObs+nPar) = SS;
1080
1081 UpperTriangularMatrix UU;
1082 QRZ(SRF, UU);
1083
1084 SS = UU.SymSubMatrix(nObs+1, nObs+nPar);
1085 UpperTriangularMatrix SH_rt = UU.SymSubMatrix(1, nObs);
1086 Matrix YY = UU.SubMatrix(1, nObs, nObs+1, nObs+nPar);
1087
1088 UpperTriangularMatrix SHi = SH_rt.i();
1089
1090 Matrix KT = SHi * YY;
1091 SymmetricMatrix Hi; Hi << SHi * SHi.t();
1092
1093 dx = KT.t() * ll;
1094 QQ << (SS.t() * SS);
1095}
1096
1097// Phase Wind-Up Correction
1098///////////////////////////////////////////////////////////////////////////
1099double bncModel::windUp(const QString& prn, const ColumnVector& rSat,
1100 const ColumnVector& rRec) {
1101
1102 double Mjd = _time.mjd() + _time.daysec() / 86400.0;
1103
1104 // First time - initialize to zero
1105 // -------------------------------
1106 if (!_windUpTime.contains(prn)) {
1107 _windUpTime[prn] = Mjd;
1108 _windUpSum[prn] = 0.0;
1109 }
1110
1111 // Compute the correction for new time
1112 // -----------------------------------
1113 else if (_windUpTime[prn] != Mjd) {
1114 _windUpTime[prn] = Mjd;
1115
1116 // Unit Vector GPS Satellite --> Receiver
1117 // --------------------------------------
1118 ColumnVector rho = rRec - rSat;
1119 rho /= rho.norm_Frobenius();
1120
1121 // GPS Satellite unit Vectors sz, sy, sx
1122 // -------------------------------------
1123 ColumnVector sz = -rSat / rSat.norm_Frobenius();
1124
1125 ColumnVector xSun = Sun(Mjd);
1126 xSun /= xSun.norm_Frobenius();
1127
1128 ColumnVector sy = crossproduct(sz, xSun);
1129 ColumnVector sx = crossproduct(sy, sz);
1130
1131 // Effective Dipole of the GPS Satellite Antenna
1132 // ---------------------------------------------
1133 ColumnVector dipSat = sx - rho * DotProduct(rho,sx)
1134 - crossproduct(rho, sy);
1135
1136 // Receiver unit Vectors rx, ry
1137 // ----------------------------
1138 ColumnVector rx(3);
1139 ColumnVector ry(3);
1140
1141 double recEll[3]; xyz2ell(rRec.data(), recEll) ;
1142 double neu[3];
1143
1144 neu[0] = 1.0;
1145 neu[1] = 0.0;
1146 neu[2] = 0.0;
1147 neu2xyz(recEll, neu, rx.data());
1148
1149 neu[0] = 0.0;
1150 neu[1] = -1.0;
1151 neu[2] = 0.0;
1152 neu2xyz(recEll, neu, ry.data());
1153
1154 // Effective Dipole of the Receiver Antenna
1155 // ----------------------------------------
1156 ColumnVector dipRec = rx - rho * DotProduct(rho,rx)
1157 + crossproduct(rho, ry);
1158
1159 // Resulting Effect
1160 // ----------------
1161 double alpha = DotProduct(dipSat,dipRec) /
1162 (dipSat.norm_Frobenius() * dipRec.norm_Frobenius());
1163
1164 if (alpha > 1.0) alpha = 1.0;
1165 if (alpha < -1.0) alpha = -1.0;
1166
1167 double dphi = acos(alpha) / 2.0 / M_PI; // in cycles
1168
1169 if ( DotProduct(rho, crossproduct(dipSat, dipRec)) < 0.0 ) {
1170 dphi = -dphi;
1171 }
1172
1173 _windUpSum[prn] = floor(_windUpSum[prn] - dphi + 0.5) + dphi;
1174 }
1175
1176 return _windUpSum[prn];
1177}
Note: See TracBrowser for help on using the repository browser.