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

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

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