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

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

* empty log message *
File size: 24.6 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
53using namespace std;
54
55const unsigned MINOBS = 4;
56const double MINELE = 10.0 * M_PI / 180.0;
57const double MAXRES_CODE = 10.0;
58const double MAXRES_PHASE = 0.10;
59const double sig_crd_0 = 100.0;
60const double sig_crd_p = 100.0;
61const double sig_clk_0 = 1000.0;
62const double sig_trp_0 = 0.01;
63const double sig_trp_p = 1e-6;
64const double sig_amb_0 = 1000.0;
65const double sig_P3 = 1.0;
66const double sig_L3 = 0.01;
67
68// Constructor
69////////////////////////////////////////////////////////////////////////////
70bncParam::bncParam(bncParam::parType typeIn, int indexIn,
71 const QString& prnIn) {
72 type = typeIn;
73 index = indexIn;
74 prn = prnIn;
75 index_old = 0;
76 xx = 0.0;
77}
78
79// Destructor
80////////////////////////////////////////////////////////////////////////////
81bncParam::~bncParam() {
82}
83
84// Partial
85////////////////////////////////////////////////////////////////////////////
86double bncParam::partial(t_satData* satData, bool phase) {
87
88 // Coordinates
89 // -----------
90 if (type == CRD_X) {
91 return (xx - satData->xx(1)) / satData->rho;
92 }
93 else if (type == CRD_Y) {
94 return (xx - satData->xx(2)) / satData->rho;
95 }
96 else if (type == CRD_Z) {
97 return (xx - satData->xx(3)) / satData->rho;
98 }
99
100 // Receiver Clocks
101 // ---------------
102 else if (type == RECCLK_GPS) {
103 if (satData->prn[0] == 'G') {
104 return 1.0;
105 }
106 else {
107 return 0.0;
108 }
109 }
110 else if (type == RECCLK_GLO) {
111 if (satData->prn[0] == 'R') {
112 return 1.0;
113 }
114 else {
115 return 0.0;
116 }
117 }
118
119 // Troposphere
120 // -----------
121 else if (type == TROPO) {
122 return 1.0 / sin(satData->eleSat);
123 }
124
125 // Ambiguities
126 // -----------
127 else if (type == AMB_L3) {
128 if (phase && satData->prn == prn) {
129 return 1.0;
130 }
131 else {
132 return 0.0;
133 }
134 }
135
136 // Default return
137 // --------------
138 return 0.0;
139}
140
141// Constructor
142////////////////////////////////////////////////////////////////////////////
143bncModel::bncModel(QByteArray staID) {
144
145 _staID = staID;
146
147 connect(this, SIGNAL(newMessage(QByteArray,bool)),
148 ((bncApp*)qApp), SLOT(slotMessage(const QByteArray,bool)));
149
150 bncSettings settings;
151
152 _static = false;
153 if ( Qt::CheckState(settings.value("pppStatic").toInt()) == Qt::Checked) {
154 _static = true;
155 }
156
157 _usePhase = false;
158 if ( Qt::CheckState(settings.value("pppUsePhase").toInt()) == Qt::Checked) {
159 _usePhase = true;
160 }
161
162 _estTropo = false;
163 if ( Qt::CheckState(settings.value("pppEstTropo").toInt()) == Qt::Checked) {
164 _estTropo = true;
165 }
166
167 _xcBanc.ReSize(4); _xcBanc = 0.0;
168 _ellBanc.ReSize(3); _ellBanc = 0.0;
169
170 if ( Qt::CheckState(settings.value("pppGLONASS").toInt()) == Qt::Checked) {
171 _useGlonass = true;
172 }
173 else {
174 _useGlonass = false;
175 }
176
177 int nextPar = 0;
178 _params.push_back(new bncParam(bncParam::CRD_X, ++nextPar, ""));
179 _params.push_back(new bncParam(bncParam::CRD_Y, ++nextPar, ""));
180 _params.push_back(new bncParam(bncParam::CRD_Z, ++nextPar, ""));
181 _params.push_back(new bncParam(bncParam::RECCLK_GPS, ++nextPar, ""));
182 if (_useGlonass) {
183 _params.push_back(new bncParam(bncParam::RECCLK_GLO, ++nextPar, ""));
184 }
185 if (_estTropo) {
186 _params.push_back(new bncParam(bncParam::TROPO, ++nextPar, ""));
187 }
188
189 unsigned nPar = _params.size();
190
191 _QQ.ReSize(nPar);
192
193 _QQ = 0.0;
194
195 for (int iPar = 1; iPar <= _params.size(); iPar++) {
196 bncParam* pp = _params[iPar-1];
197 if (pp->isCrd()) {
198 _QQ(iPar,iPar) = sig_crd_0 * sig_crd_0;
199 }
200 else if (pp->isClk()) {
201 _QQ(iPar,iPar) = sig_clk_0 * sig_clk_0;
202 }
203 else if (pp->type == bncParam::TROPO) {
204 _QQ(iPar,iPar) = sig_trp_0 * sig_trp_0;
205 }
206 }
207
208 // NMEA Output
209 // -----------
210 QString nmeaFileName = settings.value("nmeaFile").toString();
211 if (nmeaFileName.isEmpty()) {
212 _nmeaFile = 0;
213 _nmeaStream = 0;
214 }
215 else {
216 expandEnvVar(nmeaFileName);
217 _nmeaFile = new QFile(nmeaFileName);
218 if ( Qt::CheckState(settings.value("rnxAppend").toInt()) == Qt::Checked) {
219 _nmeaFile->open(QIODevice::WriteOnly | QIODevice::Append);
220 }
221 else {
222 _nmeaFile->open(QIODevice::WriteOnly);
223 }
224 _nmeaStream = new QTextStream();
225 _nmeaStream->setDevice(_nmeaFile);
226 QDateTime dateTime = QDateTime::currentDateTime().toUTC();
227 QString nmStr = "GPRMC," + dateTime.time().toString("hhmmss")
228 + ",A,,,,,,,"
229 + dateTime.date().toString("ddMMyy")
230 + ",,";
231
232 writeNMEAstr(nmStr);
233 }
234}
235
236// Destructor
237////////////////////////////////////////////////////////////////////////////
238bncModel::~bncModel() {
239 delete _nmeaStream;
240 delete _nmeaFile;
241}
242
243// Bancroft Solution
244////////////////////////////////////////////////////////////////////////////
245t_irc bncModel::cmpBancroft(t_epoData* epoData) {
246
247 if (epoData->sizeGPS() < MINOBS) {
248 _log += "\nNot enough data";
249 return failure;
250 }
251
252 Matrix BB(epoData->sizeGPS(), 4);
253
254 QMapIterator<QString, t_satData*> it(epoData->satDataGPS);
255 int iObs = 0;
256 while (it.hasNext()) {
257 ++iObs;
258 it.next();
259 QString prn = it.key();
260 t_satData* satData = it.value();
261 BB(iObs, 1) = satData->xx(1);
262 BB(iObs, 2) = satData->xx(2);
263 BB(iObs, 3) = satData->xx(3);
264 BB(iObs, 4) = satData->P3 + satData->clk;
265 }
266
267 bancroft(BB, _xcBanc);
268
269 // Ellipsoidal Coordinates
270 // ------------------------
271 xyz2ell(_xcBanc.data(), _ellBanc.data());
272
273 // Compute Satellite Elevations
274 // ----------------------------
275 QMutableMapIterator<QString, t_satData*> iGPS(epoData->satDataGPS);
276 while (iGPS.hasNext()) {
277 iGPS.next();
278 QString prn = iGPS.key();
279 t_satData* satData = iGPS.value();
280
281 ColumnVector rr = satData->xx - _xcBanc.Rows(1,3);
282 double rho = rr.norm_Frobenius();
283
284 double neu[3];
285 xyz2neu(_ellBanc.data(), rr.data(), neu);
286
287 satData->eleSat = acos( sqrt(neu[0]*neu[0] + neu[1]*neu[1]) / rho );
288 if (neu[2] < 0) {
289 satData->eleSat *= -1.0;
290 }
291 satData->azSat = atan2(neu[1], neu[0]);
292
293 if (satData->eleSat < MINELE) {
294 delete satData;
295 iGPS.remove();
296 }
297 }
298
299 QMutableMapIterator<QString, t_satData*> iGlo(epoData->satDataGlo);
300 while (iGlo.hasNext()) {
301 iGlo.next();
302 QString prn = iGlo.key();
303 t_satData* satData = iGlo.value();
304
305 ColumnVector rr = satData->xx - _xcBanc.Rows(1,3);
306 double rho = rr.norm_Frobenius();
307
308 double neu[3];
309 xyz2neu(_ellBanc.data(), rr.data(), neu);
310
311 satData->eleSat = acos( sqrt(neu[0]*neu[0] + neu[1]*neu[1]) / rho );
312 if (neu[2] < 0) {
313 satData->eleSat *= -1.0;
314 }
315 satData->azSat = atan2(neu[1], neu[0]);
316
317 if (satData->eleSat < MINELE) {
318 delete satData;
319 iGlo.remove();
320 }
321 }
322
323 return success;
324}
325
326// Computed Value
327////////////////////////////////////////////////////////////////////////////
328double bncModel::cmpValue(t_satData* satData) {
329
330 ColumnVector xRec(3);
331 xRec(1) = x();
332 xRec(2) = y();
333 xRec(3) = z();
334
335 double rho0 = (satData->xx - xRec).norm_Frobenius();
336 double dPhi = t_CST::omega * rho0 / t_CST::c;
337
338 xRec(1) = x() * cos(dPhi) - y() * sin(dPhi);
339 xRec(2) = y() * cos(dPhi) + x() * sin(dPhi);
340 xRec(3) = z();
341
342 satData->rho = (satData->xx - xRec).norm_Frobenius();
343
344 double tropDelay = delay_saast(satData->eleSat) +
345 trp() / sin(satData->eleSat);
346
347 double clk = 0.0;
348 if (satData->prn[0] == 'G') {
349 clk = clkGPS();
350 }
351 else if (satData->prn[0] == 'R') {
352 clk = clkGlo();
353 }
354
355 cout << satData->prn.toAscii().data() << " "
356 << clk << " " << satData->rho + clk - satData->clk + tropDelay << endl;
357
358 return satData->rho + clk - satData->clk + tropDelay;
359}
360
361// Tropospheric Model (Saastamoinen)
362////////////////////////////////////////////////////////////////////////////
363double bncModel::delay_saast(double Ele) {
364
365 double height = _ellBanc(3);
366
367 double pp = 1013.25 * pow(1.0 - 2.26e-5 * height, 5.225);
368 double TT = 18.0 - height * 0.0065 + 273.15;
369 double hh = 50.0 * exp(-6.396e-4 * height);
370 double ee = hh / 100.0 * exp(-37.2465 + 0.213166*TT - 0.000256908*TT*TT);
371
372 double h_km = height / 1000.0;
373
374 if (h_km < 0.0) h_km = 0.0;
375 if (h_km > 5.0) h_km = 5.0;
376 int ii = int(h_km + 1);
377 double href = ii - 1;
378
379 double bCor[6];
380 bCor[0] = 1.156;
381 bCor[1] = 1.006;
382 bCor[2] = 0.874;
383 bCor[3] = 0.757;
384 bCor[4] = 0.654;
385 bCor[5] = 0.563;
386
387 double BB = bCor[ii-1] + (bCor[ii]-bCor[ii-1]) * (h_km - href);
388
389 double zen = M_PI/2.0 - Ele;
390
391 return (0.002277/cos(zen)) * (pp + ((1255.0/TT)+0.05)*ee - BB*(tan(zen)*tan(zen)));
392}
393
394// Prediction Step of the Filter
395////////////////////////////////////////////////////////////////////////////
396void bncModel::predict(t_epoData* epoData) {
397
398 if (_usePhase) {
399
400 // Make a copy of QQ and xx, set parameter indices
401 // -----------------------------------------------
402 SymmetricMatrix QQ_old = _QQ;
403
404 for (int iPar = 1; iPar <= _params.size(); iPar++) {
405 _params[iPar-1]->index_old = _params[iPar-1]->index;
406 _params[iPar-1]->index = 0;
407 }
408
409 // Remove Ambiguity Parameters without observations
410 // ------------------------------------------------
411 int iPar = 0;
412 QMutableVectorIterator<bncParam*> it(_params);
413 while (it.hasNext()) {
414 bncParam* par = it.next();
415 bool removed = false;
416 if (par->type == bncParam::AMB_L3) {
417 if (epoData->satDataGPS.find(par->prn) == epoData->satDataGPS.end() &&
418 epoData->satDataGlo.find(par->prn) == epoData->satDataGlo.end() ) {
419 removed = true;
420 delete par;
421 it.remove();
422 }
423 }
424 if (! removed) {
425 ++iPar;
426 par->index = iPar;
427 }
428 }
429
430 // Add new ambiguity parameters
431 // ----------------------------
432 QMapIterator<QString, t_satData*> iGPS(epoData->satDataGPS);
433 while (iGPS.hasNext()) {
434 iGPS.next();
435 QString prn = iGPS.key();
436 bool found = false;
437 for (int iPar = 1; iPar <= _params.size(); iPar++) {
438 if (_params[iPar-1]->type == bncParam::AMB_L3 &&
439 _params[iPar-1]->prn == prn) {
440 found = true;
441 break;
442 }
443 }
444 if (!found) {
445 bncParam* par = new bncParam(bncParam::AMB_L3, _params.size()+1, prn);
446 _params.push_back(par);
447 }
448 }
449
450 QMapIterator<QString, t_satData*> iGlo(epoData->satDataGlo);
451 while (iGlo.hasNext()) {
452 iGlo.next();
453 QString prn = iGlo.key();
454 t_satData* satData = iGlo.value();
455 bool found = false;
456 for (int iPar = 1; iPar <= _params.size(); iPar++) {
457 if (_params[iPar-1]->type == bncParam::AMB_L3 &&
458 _params[iPar-1]->prn == prn) {
459 found = true;
460 break;
461 }
462 }
463 if (!found) {
464 bncParam* par = new bncParam(bncParam::AMB_L3, _params.size()+1, prn);
465 _params.push_back(par);
466 par->xx = satData->P3 - cmpValue(satData);
467 }
468 }
469
470 int nPar = _params.size();
471 _QQ.ReSize(nPar); _QQ = 0.0;
472 for (int i1 = 1; i1 <= nPar; i1++) {
473 bncParam* p1 = _params[i1-1];
474 if (p1->index_old != 0) {
475 _QQ(p1->index, p1->index) = QQ_old(p1->index_old, p1->index_old);
476 for (int i2 = 1; i2 <= nPar; i2++) {
477 bncParam* p2 = _params[i2-1];
478 if (p2->index_old != 0) {
479 _QQ(p1->index, p2->index) = QQ_old(p1->index_old, p2->index_old);
480 }
481 }
482 }
483 }
484
485 for (int ii = 1; ii <= nPar; ii++) {
486 bncParam* par = _params[ii-1];
487 if (par->index_old == 0) {
488 _QQ(par->index, par->index) = sig_amb_0 * sig_amb_0;
489 }
490 par->index_old = par->index;
491 }
492 }
493
494 bool firstCrd = x() == 0.0 && y() == 0.0 && z() == 0.0;
495
496 for (int iPar = 1; iPar <= _params.size(); iPar++) {
497 bncParam* pp = _params[iPar-1];
498
499 // Coordinates
500 // -----------
501 if (pp->type == bncParam::CRD_X) {
502 if (firstCrd || !_static) {
503 pp->xx = _xcBanc(1);
504 }
505 _QQ(iPar,iPar) += sig_crd_p * sig_crd_p;
506 }
507 else if (pp->type == bncParam::CRD_Y) {
508 if (firstCrd || !_static) {
509 pp->xx = _xcBanc(2);
510 }
511 _QQ(iPar,iPar) += sig_crd_p * sig_crd_p;
512 }
513 else if (pp->type == bncParam::CRD_Z) {
514 if (firstCrd || !_static) {
515 pp->xx = _xcBanc(3);
516 }
517 _QQ(iPar,iPar) += sig_crd_p * sig_crd_p;
518 }
519
520 // Receiver Clocks
521 // ---------------
522 else if (pp->isClk()) {
523 pp->xx = _xcBanc(4);
524 for (int jj = 1; jj <= _params.size(); jj++) {
525 _QQ(iPar, jj) = 0.0;
526 }
527 _QQ(iPar,iPar) = sig_clk_0 * sig_clk_0;
528 }
529
530 // Tropospheric Delay
531 // ------------------
532 else if (pp->type == bncParam::TROPO) {
533 _QQ(iPar,iPar) += sig_trp_p * sig_trp_p;
534 }
535 }
536}
537
538// Update Step of the Filter (currently just a single-epoch solution)
539////////////////////////////////////////////////////////////////////////////
540t_irc bncModel::update(t_epoData* epoData) {
541
542 _log = "Precise Point Positioning";
543
544 _time = epoData->tt;
545
546 SymmetricMatrix QQsav;
547 ColumnVector dx;
548 ColumnVector vv;
549
550 // Loop over all outliers
551 // ----------------------
552 do {
553
554 // Bancroft Solution
555 // -----------------
556 if (cmpBancroft(epoData) != success) {
557 _log += "\nBancroft failed";
558 emit newMessage(_log, false);
559 return failure;
560 }
561
562 if (epoData->sizeGPS() < MINOBS) {
563 _log += "\nNot enough data";
564 emit newMessage(_log, false);
565 return failure;
566 }
567
568 // Status Prediction
569 // -----------------
570 predict(epoData);
571
572 // Create First-Design Matrix
573 // --------------------------
574 unsigned nPar = _params.size();
575 unsigned nObs = 0;
576 if (_usePhase) {
577 nObs = 2 * epoData->sizeGPS() + epoData->sizeGlo();
578 }
579 else {
580 nObs = epoData->sizeGPS(); // Glonass pseudoranges are not used
581 }
582
583 Matrix AA(nObs, nPar); // first design matrix
584 ColumnVector ll(nObs); // tems observed-computed
585 SymmetricMatrix PP(nObs); PP = 0.0;
586
587 unsigned iObs = 0;
588
589 // GPS code and (optionally) phase observations
590 // --------------------------------------------
591 QMapIterator<QString, t_satData*> itGPS(epoData->satDataGPS);
592 while (itGPS.hasNext()) {
593 ++iObs;
594 itGPS.next();
595 QString prn = itGPS.key();
596 t_satData* satData = itGPS.value();
597
598 double rhoCmp = cmpValue(satData);
599
600 double ellWgtCoeff = 1.0;
601 //// double eleD = satData->eleSat * 180.0 / M_PI;
602 //// if (eleD < 25.0) {
603 //// ellWgtCoeff = 2.5 - (eleD - 10.0) * 0.1;
604 //// ellWgtCoeff *= ellWgtCoeff;
605 //// }
606
607 ll(iObs) = satData->P3 - rhoCmp;
608 PP(iObs,iObs) = 1.0 / (sig_P3 * sig_P3) / ellWgtCoeff;
609 for (int iPar = 1; iPar <= _params.size(); iPar++) {
610 AA(iObs, iPar) = _params[iPar-1]->partial(satData, false);
611 }
612
613 if (_usePhase) {
614 ++iObs;
615 ll(iObs) = satData->L3 - rhoCmp;
616 PP(iObs,iObs) = 1.0 / (sig_L3 * sig_L3) / ellWgtCoeff;
617 for (int iPar = 1; iPar <= _params.size(); iPar++) {
618 if (_params[iPar-1]->type == bncParam::AMB_L3 &&
619 _params[iPar-1]->prn == prn) {
620 ll(iObs) -= _params[iPar-1]->xx;
621 }
622 AA(iObs, iPar) = _params[iPar-1]->partial(satData, true);
623 }
624 }
625 }
626
627 // Glonass phase observations
628 // --------------------------
629 if (_usePhase) {
630 QMapIterator<QString, t_satData*> itGlo(epoData->satDataGlo);
631 while (itGlo.hasNext()) {
632 ++iObs;
633 itGlo.next();
634 QString prn = itGlo.key();
635 t_satData* satData = itGlo.value();
636
637 double rhoCmp = cmpValue(satData);
638
639 double ellWgtCoeff = 1.0;
640 //// double eleD = satData->eleSat * 180.0 / M_PI;
641 //// if (eleD < 25.0) {
642 //// ellWgtCoeff = 2.5 - (eleD - 10.0) * 0.1;
643 //// ellWgtCoeff *= ellWgtCoeff;
644 //// }
645
646 ll(iObs) = satData->L3 - rhoCmp;
647 PP(iObs,iObs) = 1.0 / (sig_L3 * sig_L3) / ellWgtCoeff;
648 for (int iPar = 1; iPar <= _params.size(); iPar++) {
649 if (_params[iPar-1]->type == bncParam::AMB_L3 &&
650 _params[iPar-1]->prn == prn) {
651 ll(iObs) -= _params[iPar-1]->xx;
652 }
653 AA(iObs, iPar) = _params[iPar-1]->partial(satData, true);
654 }
655
656 //// beg test
657 //double rhoCmp = cmpValue(satData);
658 //cout.setf(ios::fixed);
659 //cout << prn.toAscii().data() << " "
660 // << setprecision(3) << rhoCmp << " "
661 // << setprecision(3) << satData->P3 << " "
662 // << setprecision(3) << satData->L3 << " " << endl;
663 //
664 ////// end test
665 }
666 }
667
668 // Compute Filter Update
669 // ---------------------
670 QQsav = _QQ;
671
672 Matrix ATP = AA.t() * PP;
673 SymmetricMatrix NN = _QQ.i();
674 NN << NN + ATP * AA;
675 _QQ = NN.i();
676 dx = _QQ * ATP * ll;
677 vv = ll - AA * dx;
678
679 } while (outlierDetection(QQsav, vv, epoData->satDataGPS,
680 epoData->satDataGlo) != 0);
681
682 // Set Solution Vector
683 // -------------------
684 ostringstream str1;
685 str1.setf(ios::fixed);
686 QVectorIterator<bncParam*> itPar(_params);
687 while (itPar.hasNext()) {
688 bncParam* par = itPar.next();
689 par->xx += dx(par->index);
690 if (par->type == bncParam::RECCLK_GPS) {
691 str1 << "\n clk GPS = " << setw(6) << setprecision(3) << par->xx
692 << " +- " << setw(6) << setprecision(3)
693 << sqrt(_QQ(par->index,par->index));
694 }
695 if (par->type == bncParam::RECCLK_GLO) {
696 str1 << "\n clk GLO = " << setw(6) << setprecision(3) << par->xx
697 << " +- " << setw(6) << setprecision(3)
698 << sqrt(_QQ(par->index,par->index));
699 }
700 else if (par->type == bncParam::AMB_L3) {
701 str1 << "\n amb " << par->prn.toAscii().data() << " = "
702 << setw(6) << setprecision(3) << par->xx
703 << " +- " << setw(6) << setprecision(3)
704 << sqrt(_QQ(par->index,par->index));
705 }
706 else if (par->type == bncParam::TROPO) {
707 str1 << "\n trp = " << par->prn.toAscii().data()
708 << setw(7) << setprecision(3) << delay_saast(M_PI/2.0) << " "
709 << setw(6) << setprecision(3) << showpos << par->xx << noshowpos
710 << " +- " << setw(6) << setprecision(3)
711 << sqrt(_QQ(par->index,par->index));
712 }
713 }
714 _log += str1.str().c_str();
715
716 // Message (both log file and screen)
717 // ----------------------------------
718 ostringstream str2;
719 str2.setf(ios::fixed);
720 str2 << _staID.data() << ": PPP "
721 << epoData->tt.timestr(1) << " " << epoData->sizeAll() << " "
722 << setw(14) << setprecision(3) << x() << " +- "
723 << setw(6) << setprecision(3) << sqrt(_QQ(1,1)) << " "
724 << setw(14) << setprecision(3) << y() << " +- "
725 << setw(6) << setprecision(3) << sqrt(_QQ(2,2)) << " "
726 << setw(14) << setprecision(3) << z() << " +- "
727 << setw(6) << setprecision(3) << sqrt(_QQ(3,3));
728
729 emit newMessage(_log, false);
730 emit newMessage(QByteArray(str2.str().c_str()), true);
731
732 // NMEA Output
733 // -----------
734 double xyz[3];
735 xyz[0] = x();
736 xyz[1] = y();
737 xyz[2] = z();
738 double ell[3];
739 xyz2ell(xyz, ell);
740 double phiDeg = ell[0] * 180 / M_PI;
741 double lamDeg = ell[1] * 180 / M_PI;
742
743 char phiCh = 'N';
744 if (phiDeg < 0) {
745 phiDeg = -phiDeg;
746 phiCh = 'S';
747 }
748 char lamCh = 'E';
749 if (lamDeg < 0) {
750 lamDeg = -lamDeg;
751 lamCh = 'W';
752 }
753
754 double dop = 2.0; // TODO
755
756 ostringstream str3;
757 str3.setf(ios::fixed);
758 str3 << "GPGGA,"
759 << epoData->tt.timestr(0,0) << ','
760 << setw(2) << setfill('0') << int(phiDeg)
761 << setw(10) << setprecision(7) << setfill('0')
762 << fmod(60*phiDeg,60) << ',' << phiCh << ','
763 << setw(2) << setfill('0') << int(lamDeg)
764 << setw(10) << setprecision(7) << setfill('0')
765 << fmod(60*lamDeg,60) << ',' << lamCh
766 << ",1," << setw(2) << setfill('0') << epoData->sizeAll() << ','
767 << setw(3) << setprecision(1) << dop << ','
768 << setprecision(3) << ell[2] << ",M,0.0,M,,,";
769
770 writeNMEAstr(QString(str3.str().c_str()));
771
772 return success;
773}
774
775// Outlier Detection
776////////////////////////////////////////////////////////////////////////////
777int bncModel::outlierDetection(const SymmetricMatrix& QQsav,
778 const ColumnVector& vv,
779 QMap<QString, t_satData*>& satDataGPS,
780 QMap<QString, t_satData*>& satDataGlo) {
781
782 double vvMaxCodeGPS = 0.0;
783 double vvMaxPhaseGPS = 0.0;
784 double vvMaxPhaseGlo = 0.0;
785 QMutableMapIterator<QString, t_satData*> itMaxCodeGPS(satDataGPS);
786 QMutableMapIterator<QString, t_satData*> itMaxPhaseGPS(satDataGPS);
787 QMutableMapIterator<QString, t_satData*> itMaxPhaseGlo(satDataGlo);
788
789 int ii = 0;
790
791 // GPS code and (optionally) phase residuals
792 // -----------------------------------------
793 QMutableMapIterator<QString, t_satData*> itGPS(satDataGPS);
794 while (itGPS.hasNext()) {
795 itGPS.next();
796 ++ii;
797
798 if (vvMaxCodeGPS == 0.0 || fabs(vv(ii)) > vvMaxCodeGPS) {
799 vvMaxCodeGPS = fabs(vv(ii));
800 itMaxCodeGPS = itGPS;
801 }
802
803 if (_usePhase) {
804 ++ii;
805 if (vvMaxPhaseGPS == 0.0 || fabs(vv(ii)) > vvMaxPhaseGPS) {
806 vvMaxPhaseGPS = fabs(vv(ii));
807 itMaxPhaseGPS = itGPS;
808 }
809 }
810 }
811
812 // Glonass phase residuals
813 // -----------------------
814 if (_usePhase) {
815 QMutableMapIterator<QString, t_satData*> itGlo(satDataGlo);
816 while (itGlo.hasNext()) {
817 itGlo.next();
818 ++ii;
819 if (vvMaxPhaseGlo == 0.0 || fabs(vv(ii)) > vvMaxPhaseGlo) {
820 vvMaxPhaseGlo = fabs(vv(ii));
821 itMaxPhaseGlo = itGlo;
822 }
823 }
824 }
825
826 if (vvMaxCodeGPS > MAXRES_CODE) {
827 QString prn = itMaxCodeGPS.key();
828 t_satData* satData = itMaxCodeGPS.value();
829 delete satData;
830 itMaxCodeGPS.remove();
831 _QQ = QQsav;
832
833 _log += "\nOutlier Code " + prn.toAscii() + " "
834 + QByteArray::number(vvMaxCodeGPS, 'f', 3);
835
836 return 1;
837 }
838 else if (vvMaxPhaseGPS > MAXRES_PHASE) {
839 QString prn = itMaxPhaseGPS.key();
840 t_satData* satData = itMaxPhaseGPS.value();
841 delete satData;
842 itMaxPhaseGPS.remove();
843 _QQ = QQsav;
844
845 _log += "\nOutlier Phase " + prn.toAscii() + " "
846 + QByteArray::number(vvMaxPhaseGPS, 'f', 3);
847
848 return 1;
849 }
850 else if (vvMaxPhaseGlo > MAXRES_PHASE) {
851 QString prn = itMaxPhaseGlo.key();
852 t_satData* satData = itMaxPhaseGlo.value();
853 delete satData;
854 itMaxPhaseGlo.remove();
855 _QQ = QQsav;
856
857 _log += "\nOutlier Phase " + prn.toAscii() + " "
858 + QByteArray::number(vvMaxPhaseGlo, 'f', 3);
859
860 return 1;
861 }
862
863 return 0;
864}
865
866//
867////////////////////////////////////////////////////////////////////////////
868void bncModel::writeNMEAstr(const QString& nmStr) {
869
870 unsigned char XOR = 0;
871 for (int ii = 0; ii < nmStr.length(); ii++) {
872 XOR ^= (unsigned char) nmStr[ii].toAscii();
873 }
874
875 QString outStr = '$' + nmStr
876 + QString("*%1\n").arg(int(XOR), 0, 16).toUpper();
877
878 if (_nmeaStream) {
879 *_nmeaStream << outStr;
880 _nmeaStream->flush();
881 }
882
883 emit newNMEAstr(outStr.toAscii());
884}
Note: See TracBrowser for help on using the repository browser.