Context Navigation

source: ntrip/trunk/BNC/src/bncmodel.cpp@ 5794

Last change on this file since 5794 was 5794, checked in by mervart, 10 years ago

File size: 39.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 "bncpppclient.h"
48	#include "bnccore.h"
49	#include "bncpppclient.h"
50	#include "bancroft.h"
51	#include "bncutils.h"
52	#include "bnctides.h"
53	#include "bncantex.h"
54	#include "pppopt.h"
55
56	using namespace std;
57
58	const unsigned MINOBS = 5;
59	const double MINELE = 10.0 * M_PI / 180.0;
60	const double MAXRES_CODE = 15.0;
61	const double MAXRES_PHASE_GPS = 0.04;
62	const double MAXRES_PHASE_GLONASS = 0.08;
63	const double GLONASS_WEIGHT_FACTOR = 5.0;
64
65	// Constructor
66	////////////////////////////////////////////////////////////////////////////
67	bncParam::bncParam(bncParam::parType typeIn, int indexIn,
68	const QString& prnIn) {
69	type = typeIn;
70	index = indexIn;
71	prn = prnIn;
72	index_old = 0;
73	xx = 0.0;
74	numEpo = 0;
75	}
76
77	// Destructor
78	////////////////////////////////////////////////////////////////////////////
79	bncParam::~bncParam() {
80	}
81
82	// Partial
83	////////////////////////////////////////////////////////////////////////////
84	double bncParam::partial(t_satData* satData, bool phase) {
85
86	Tracer tracer("bncParam::partial");
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) {
103	return 1.0;
104	}
105
106	// Troposphere
107	// -----------
108	else if (type == TROPO) {
109	return 1.0 / sin(satData->eleSat);
110	}
111
112	// Glonass Offset
113	// --------------
114	else if (type == GLONASS_OFFSET) {
115	if (satData->prn[0] == 'R') {
116	return 1.0;
117	}
118	else {
119	return 0.0;
120	}
121	}
122
123	// Galileo Offset
124	// --------------
125	else if (type == GALILEO_OFFSET) {
126	if (satData->prn[0] == 'E') {
127	return 1.0;
128	}
129	else {
130	return 0.0;
131	}
132	}
133
134	// Ambiguities
135	// -----------
136	else if (type == AMB_L3) {
137	if (phase && satData->prn == prn) {
138	return 1.0;
139	}
140	else {
141	return 0.0;
142	}
143	}
144
145	// Default return
146	// --------------
147	return 0.0;
148	}
149
150	// Constructor
151	////////////////////////////////////////////////////////////////////////////
152	bncModel::bncModel(bncPPPclient* pppClient) {
153
154	_pppClient = pppClient;
155	_staID = pppClient->staID();
156	_opt = pppClient->opt();
157
158	// NMEA Output
159	// -----------
160	if (_opt->nmeaFile.isEmpty()) {
161	_nmeaFile = 0;
162	_nmeaStream = 0;
163	}
164	else {
165	QString hlpName = _opt->nmeaFile; expandEnvVar(hlpName);
166	_nmeaFile = new QFile(hlpName);
167	if (_opt->rnxAppend) {
168	_nmeaFile->open(QIODevice::WriteOnly \| QIODevice::Append);
169	}
170	else {
171	_nmeaFile->open(QIODevice::WriteOnly);
172	}
173	_nmeaStream = new QTextStream();
174	_nmeaStream->setDevice(_nmeaFile);
175	}
176
177	// Antenna Name, ANTEX File
178	// ------------------------
179	_antex = 0;
180	if (!_opt->antexFile.isEmpty()) {
181	_antex = new bncAntex();
182	if (_antex->readFile(_opt->antexFile) != success) {
183	_pppClient->emitNewMessage("wrong ANTEX file", true);
184	delete _antex;
185	_antex = 0;
186	}
187	}
188
189	// Bancroft Coordinates
190	// --------------------
191	_xcBanc.ReSize(4); _xcBanc = 0.0;
192	_ellBanc.ReSize(3); _ellBanc = 0.0;
193
194	// Save copy of data (used in outlier detection)
195	// ---------------------------------------------
196	_epoData_sav = new t_epoData();
197	}
198
199	// Destructor
200	////////////////////////////////////////////////////////////////////////////
201	bncModel::~bncModel() {
202	delete _nmeaStream;
203	delete _nmeaFile;
204	for (int ii = 0; ii < _posAverage.size(); ++ii) {
205	delete _posAverage[ii];
206	}
207	delete _antex;
208	for (int iPar = 1; iPar <= _params.size(); iPar++) {
209	delete _params[iPar-1];
210	}
211	for (int iPar = 1; iPar <= _params_sav.size(); iPar++) {
212	delete _params_sav[iPar-1];
213	}
214	delete _epoData_sav;
215	}
216
217	// Reset Parameters and Variance-Covariance Matrix
218	////////////////////////////////////////////////////////////////////////////
219	void bncModel::reset() {
220
221	Tracer tracer("bncModel::reset");
222
223	double lastTrp = 0.0;
224	for (int ii = 0; ii < _params.size(); ii++) {
225	bncParam* pp = _params[ii];
226	if (pp->type == bncParam::TROPO) {
227	lastTrp = pp->xx;
228	}
229	delete pp;
230	}
231	_params.clear();
232
233	int nextPar = 0;
234	_params.push_back(new bncParam(bncParam::CRD_X, ++nextPar, ""));
235	_params.push_back(new bncParam(bncParam::CRD_Y, ++nextPar, ""));
236	_params.push_back(new bncParam(bncParam::CRD_Z, ++nextPar, ""));
237	_params.push_back(new bncParam(bncParam::RECCLK, ++nextPar, ""));
238	if (_opt->estTropo) {
239	_params.push_back(new bncParam(bncParam::TROPO, ++nextPar, ""));
240	}
241	if (_opt->useGlonass) {
242	_params.push_back(new bncParam(bncParam::GLONASS_OFFSET, ++nextPar, ""));
243	}
244	if (_opt->useGalileo) {
245	_params.push_back(new bncParam(bncParam::GALILEO_OFFSET, ++nextPar, ""));
246	}
247
248	_QQ.ReSize(_params.size());
249	_QQ = 0.0;
250	for (int iPar = 1; iPar <= _params.size(); iPar++) {
251	bncParam* pp = _params[iPar-1];
252	pp->xx = 0.0;
253	if (pp->isCrd()) {
254	_QQ(iPar,iPar) = _opt->sigCrd0 * _opt->sigCrd0;
255	}
256	else if (pp->type == bncParam::RECCLK) {
257	_QQ(iPar,iPar) = _opt->sigClk0 * _opt->sigClk0;
258	}
259	else if (pp->type == bncParam::TROPO) {
260	_QQ(iPar,iPar) = _opt->sigTrp0 * _opt->sigTrp0;
261	pp->xx = lastTrp;
262	}
263	else if (pp->type == bncParam::GLONASS_OFFSET) {
264	_QQ(iPar,iPar) = _opt->sigGlonassOffset0 * _opt->sigGlonassOffset0;
265	}
266	else if (pp->type == bncParam::GALILEO_OFFSET) {
267	_QQ(iPar,iPar) = _opt->sigGalileoOffset0 * _opt->sigGalileoOffset0;
268	}
269	}
270	}
271
272	// Bancroft Solution
273	////////////////////////////////////////////////////////////////////////////
274	t_irc bncModel::cmpBancroft(t_epoData* epoData) {
275
276	Tracer tracer("bncModel::cmpBancroft");
277
278	if (epoData->sizeSys('G') < MINOBS) {
279	_log += "bncModel::cmpBancroft: not enough data\n";
280	return failure;
281	}
282
283	Matrix BB(epoData->sizeSys('G'), 4);
284
285	QMapIterator<QString, t_satData*> it(epoData->satData);
286	int iObsBanc = 0;
287	while (it.hasNext()) {
288	it.next();
289	t_satData* satData = it.value();
290	if (satData->system() == 'G') {
291	++iObsBanc;
292	QString prn = it.key();
293	BB(iObsBanc, 1) = satData->xx(1);
294	BB(iObsBanc, 2) = satData->xx(2);
295	BB(iObsBanc, 3) = satData->xx(3);
296	BB(iObsBanc, 4) = satData->P3 + satData->clk;
297	}
298	}
299
300	bancroft(BB, _xcBanc);
301
302	// Ellipsoidal Coordinates
303	// ------------------------
304	xyz2ell(_xcBanc.data(), _ellBanc.data());
305
306	// Compute Satellite Elevations
307	// ----------------------------
308	QMutableMapIterator<QString, t_satData*> im(epoData->satData);
309	while (im.hasNext()) {
310	im.next();
311	t_satData* satData = im.value();
312	cmpEle(satData);
313	if (satData->eleSat < MINELE) {
314	delete satData;
315	im.remove();
316	}
317	}
318
319	return success;
320	}
321
322	// Computed Value
323	////////////////////////////////////////////////////////////////////////////
324	double bncModel::cmpValue(t_satData* satData, bool phase) {
325
326	Tracer tracer("bncModel::cmpValue");
327
328	ColumnVector xRec(3);
329	xRec(1) = x();
330	xRec(2) = y();
331	xRec(3) = z();
332
333	double rho0 = (satData->xx - xRec).norm_Frobenius();
334	double dPhi = t_CST::omega * rho0 / t_CST::c;
335
336	xRec(1) = x() * cos(dPhi) - y() * sin(dPhi);
337	xRec(2) = y() * cos(dPhi) + x() * sin(dPhi);
338	xRec(3) = z();
339
340	tides(_time, xRec);
341
342	satData->rho = (satData->xx - xRec).norm_Frobenius();
343
344	double tropDelay = delay_saast(xRec, satData->eleSat) +
345	trp() / sin(satData->eleSat);
346
347	double wind = 0.0;
348	if (phase) {
349	wind = windUp(satData->prn, satData->xx, xRec) * satData->lambda3;
350	}
351
352	double offset = 0.0;
353	if (satData->prn[0] == 'R') {
354	offset = Glonass_offset();
355	}
356	else if (satData->prn[0] == 'E') {
357	offset = Galileo_offset();
358	}
359
360	double phaseCenter = 0.0;
361	if (_antex) {
362	bool found;
363	phaseCenter = _antex->pco(_opt->antennaName, satData->eleSat, found);
364	if (!found) {
365	_pppClient->emitNewMessage("ANTEX: antenna >"
366	+ _opt->antennaName.toAscii() + "< not found", true);
367	}
368	}
369
370	double antennaOffset = 0.0;
371	if (_opt->antEccSet()) {
372	double cosa = cos(satData->azSat);
373	double sina = sin(satData->azSat);
374	double cose = cos(satData->eleSat);
375	double sine = sin(satData->eleSat);
376	antennaOffset = -_opt->antEccNEU[0] * cosa*cose
377	-_opt->antEccNEU[1] * sina*cose
378	-_opt->antEccNEU[2] * sine;
379	}
380
381	return satData->rho + phaseCenter + antennaOffset + clk()
382	+ offset - satData->clk + tropDelay + wind;
383	}
384
385	// Tropospheric Model (Saastamoinen)
386	////////////////////////////////////////////////////////////////////////////
387	double bncModel::delay_saast(const ColumnVector& xyz, double Ele) {
388
389	Tracer tracer("bncModel::delay_saast");
390
391	if (xyz[0] == 0.0 && xyz[1] == 0.0 && xyz[2] == 0.0) {
392	return 0.0;
393	}
394
395	double ell[3];
396	xyz2ell(xyz.data(), ell);
397	double height = ell[2];
398
399	double pp = 1013.25 * pow(1.0 - 2.26e-5 * height, 5.225);
400	double TT = 18.0 - height * 0.0065 + 273.15;
401	double hh = 50.0 * exp(-6.396e-4 * height);
402	double ee = hh / 100.0 * exp(-37.2465 + 0.213166TT - 0.000256908TT*TT);
403
404	double h_km = height / 1000.0;
405
406	if (h_km < 0.0) h_km = 0.0;
407	if (h_km > 5.0) h_km = 5.0;
408	int ii = int(h_km + 1);
409	double href = ii - 1;
410
411	double bCor[6];
412	bCor[0] = 1.156;
413	bCor[1] = 1.006;
414	bCor[2] = 0.874;
415	bCor[3] = 0.757;
416	bCor[4] = 0.654;
417	bCor[5] = 0.563;
418
419	double BB = bCor[ii-1] + (bCor[ii]-bCor[ii-1]) * (h_km - href);
420
421	double zen = M_PI/2.0 - Ele;
422
423	return (0.002277/cos(zen)) * (pp + ((1255.0/TT)+0.05)ee - BB(tan(zen)*tan(zen)));
424	}
425
426	// Prediction Step of the Filter
427	////////////////////////////////////////////////////////////////////////////
428	void bncModel::predict(int iPhase, t_epoData* epoData) {
429
430	Tracer tracer("bncModel::predict");
431
432	if (iPhase == 0) {
433
434	bool firstCrd = false;
435	if (!_lastTimeOK.valid() \|\| (_opt->maxSolGap > 0 && _time - _lastTimeOK > _opt->maxSolGap)) {
436	firstCrd = true;
437	_startTime = epoData->tt;
438	reset();
439	}
440
441	// Use different white noise for Quick-Start mode
442	// ----------------------------------------------
443	double sigCrdP_used = _opt->sigCrdP;
444	if ( _opt->quickStart > 0.0 && _opt->quickStart > (epoData->tt - _startTime) ) {
445	sigCrdP_used = 0.0;
446	}
447
448	// Predict Parameter values, add white noise
449	// -----------------------------------------
450	for (int iPar = 1; iPar <= _params.size(); iPar++) {
451	bncParam* pp = _params[iPar-1];
452
453	// Coordinates
454	// -----------
455	if (pp->type == bncParam::CRD_X) {
456	if (firstCrd) {
457	if (_opt->refCrdSet()) {
458	pp->xx = _opt->refCrd[0];
459	}
460	else {
461	pp->xx = _xcBanc(1);
462	}
463	}
464	_QQ(iPar,iPar) += sigCrdP_used * sigCrdP_used;
465	}
466	else if (pp->type == bncParam::CRD_Y) {
467	if (firstCrd) {
468	if (_opt->refCrdSet()) {
469	pp->xx = _opt->refCrd[1];
470	}
471	else {
472	pp->xx = _xcBanc(2);
473	}
474	}
475	_QQ(iPar,iPar) += sigCrdP_used * sigCrdP_used;
476	}
477	else if (pp->type == bncParam::CRD_Z) {
478	if (firstCrd) {
479	if (_opt->refCrdSet()) {
480	pp->xx = _opt->refCrd[2];
481	}
482	else {
483	pp->xx = _xcBanc(3);
484	}
485	}
486	_QQ(iPar,iPar) += sigCrdP_used * sigCrdP_used;
487	}
488
489	// Receiver Clocks
490	// ---------------
491	else if (pp->type == bncParam::RECCLK) {
492	pp->xx = _xcBanc(4);
493	for (int jj = 1; jj <= _params.size(); jj++) {
494	_QQ(iPar, jj) = 0.0;
495	}
496	_QQ(iPar,iPar) = _opt->sigClk0 * _opt->sigClk0;
497	}
498
499	// Tropospheric Delay
500	// ------------------
501	else if (pp->type == bncParam::TROPO) {
502	_QQ(iPar,iPar) += _opt->sigTrpP * _opt->sigTrpP;
503	}
504
505	// Glonass Offset
506	// --------------
507	else if (pp->type == bncParam::GLONASS_OFFSET) {
508	bool epoSpec = true;
509	if (epoSpec) {
510	pp->xx = 0.0;
511	for (int jj = 1; jj <= _params.size(); jj++) {
512	_QQ(iPar, jj) = 0.0;
513	}
514	_QQ(iPar,iPar) = _opt->sigGlonassOffset0 * _opt->sigGlonassOffset0;
515	}
516	else {
517	_QQ(iPar,iPar) += _opt->sigGlonassOffsetP * _opt->sigGlonassOffsetP;
518	}
519	}
520
521	// Galileo Offset
522	// --------------
523	else if (pp->type == bncParam::GALILEO_OFFSET) {
524	_QQ(iPar,iPar) += _opt->sigGalileoOffsetP * _opt->sigGalileoOffsetP;
525	}
526	}
527	}
528
529	// Add New Ambiguities if necessary
530	// --------------------------------
531	if (_opt->usePhase) {
532
533	// Make a copy of QQ and xx, set parameter indices
534	// -----------------------------------------------
535	SymmetricMatrix QQ_old = _QQ;
536
537	for (int iPar = 1; iPar <= _params.size(); iPar++) {
538	_params[iPar-1]->index_old = _params[iPar-1]->index;
539	_params[iPar-1]->index = 0;
540	}
541
542	// Remove Ambiguity Parameters without observations
543	// ------------------------------------------------
544	int iPar = 0;
545	QMutableVectorIterator<bncParam*> im(_params);
546	while (im.hasNext()) {
547	bncParam* par = im.next();
548	bool removed = false;
549	if (par->type == bncParam::AMB_L3) {
550	if (epoData->satData.find(par->prn) == epoData->satData.end()) {
551	removed = true;
552	delete par;
553	im.remove();
554	}
555	}
556	if (! removed) {
557	++iPar;
558	par->index = iPar;
559	}
560	}
561
562	// Add new ambiguity parameters
563	// ----------------------------
564	QMapIterator<QString, t_satData*> it(epoData->satData);
565	while (it.hasNext()) {
566	it.next();
567	t_satData* satData = it.value();
568	addAmb(satData);
569	}
570
571	int nPar = _params.size();
572	_QQ.ReSize(nPar); _QQ = 0.0;
573	for (int i1 = 1; i1 <= nPar; i1++) {
574	bncParam* p1 = _params[i1-1];
575	if (p1->index_old != 0) {
576	_QQ(p1->index, p1->index) = QQ_old(p1->index_old, p1->index_old);
577	for (int i2 = 1; i2 <= nPar; i2++) {
578	bncParam* p2 = _params[i2-1];
579	if (p2->index_old != 0) {
580	_QQ(p1->index, p2->index) = QQ_old(p1->index_old, p2->index_old);
581	}
582	}
583	}
584	}
585
586	for (int ii = 1; ii <= nPar; ii++) {
587	bncParam* par = _params[ii-1];
588	if (par->index_old == 0) {
589	_QQ(par->index, par->index) = _opt->sigAmb0 * _opt->sigAmb0;
590	}
591	par->index_old = par->index;
592	}
593	}
594	}
595
596	// Update Step of the Filter (currently just a single-epoch solution)
597	////////////////////////////////////////////////////////////////////////////
598	t_irc bncModel::update(t_epoData* epoData) {
599
600	Tracer tracer("bncModel::update");
601
602	_log.clear();
603
604	_time = epoData->tt; // current epoch time
605
606	if (_opt->pppMode) {
607	_log += "Precise Point Positioning of Epoch "
608	+ QByteArray(_time.timestr(1).c_str()) +
609	"\n---------------------------------------------------------------\n";
610	}
611	else {
612	_log += "Single Point Positioning of Epoch "
613	+ QByteArray(_time.timestr(1).c_str()) +
614	"\n--------------------------------------------------------------\n";
615	}
616
617	// Outlier Detection Loop
618	// ----------------------
619	if (update_p(epoData) != success) {
620	_pppClient->emitNewMessage(_log, false);
621	return failure;
622	}
623
624	// Remember the Epoch-specific Results for the computation of means
625	// ----------------------------------------------------------------
626	pppPos* newPos = new pppPos;
627	newPos->time = epoData->tt;
628
629	// Set Solution Vector
630	// -------------------
631	ostringstream strB;
632	strB.setf(ios::fixed);
633	QVectorIterator<bncParam*> itPar(_params);
634	while (itPar.hasNext()) {
635	bncParam* par = itPar.next();
636
637	if (par->type == bncParam::RECCLK) {
638	strB << "\n clk = " << setw(10) << setprecision(3) << par->xx
639	<< " +- " << setw(6) << setprecision(3)
640	<< sqrt(_QQ(par->index,par->index));
641	}
642	else if (par->type == bncParam::AMB_L3) {
643	++par->numEpo;
644	strB << "\n amb " << par->prn.toAscii().data() << " = "
645	<< setw(10) << setprecision(3) << par->xx
646	<< " +- " << setw(6) << setprecision(3)
647	<< sqrt(_QQ(par->index,par->index))
648	<< " nEpo = " << par->numEpo;
649	}
650	else if (par->type == bncParam::TROPO) {
651	ColumnVector xyz(3); xyz(1) = x(); xyz(2) = y(); xyz(3) = z();
652	double aprTrp = delay_saast(xyz, M_PI/2.0);
653	strB << "\n trp = " << par->prn.toAscii().data()
654	<< setw(7) << setprecision(3) << aprTrp << " "
655	<< setw(6) << setprecision(3) << showpos << par->xx << noshowpos
656	<< " +- " << setw(6) << setprecision(3)
657	<< sqrt(_QQ(par->index,par->index));
658	newPos->xnt[6] = aprTrp + par->xx;
659	}
660	else if (par->type == bncParam::GLONASS_OFFSET) {
661	strB << "\n offGlo = " << setw(10) << setprecision(3) << par->xx
662	<< " +- " << setw(6) << setprecision(3)
663	<< sqrt(_QQ(par->index,par->index));
664	}
665	else if (par->type == bncParam::GALILEO_OFFSET) {
666	strB << "\n offGal = " << setw(10) << setprecision(3) << par->xx
667	<< " +- " << setw(6) << setprecision(3)
668	<< sqrt(_QQ(par->index,par->index));
669	}
670	}
671	strB << '\n';
672	_log += strB.str().c_str();
673	_pppClient->emitNewMessage(_log, false);
674
675	// Final Message (both log file and screen)
676	// ----------------------------------------
677	ostringstream strC;
678	strC.setf(ios::fixed);
679	strC << _staID.data() << " PPP "
680	<< epoData->tt.timestr(1) << " " << epoData->sizeAll() << " "
681	<< setw(14) << setprecision(3) << x() << " +- "
682	<< setw(6) << setprecision(3) << sqrt(_QQ(1,1)) << " "
683	<< setw(14) << setprecision(3) << y() << " +- "
684	<< setw(6) << setprecision(3) << sqrt(_QQ(2,2)) << " "
685	<< setw(14) << setprecision(3) << z() << " +- "
686	<< setw(6) << setprecision(3) << sqrt(_QQ(3,3));
687
688	// NEU Output
689	// ----------
690	if (_opt->refCrdSet()) {
691	newPos->xnt[0] = x() - _opt->refCrd[0];
692	newPos->xnt[1] = y() - _opt->refCrd[1];
693	newPos->xnt[2] = z() - _opt->refCrd[2];
694
695	double ellRef[3];
696	xyz2ell(_opt->refCrd, ellRef);
697	xyz2neu(ellRef, newPos->xnt, &newPos->xnt[3]);
698
699	strC << " NEU "
700	<< setw(8) << setprecision(3) << newPos->xnt[3] << " "
701	<< setw(8) << setprecision(3) << newPos->xnt[4] << " "
702	<< setw(8) << setprecision(3) << newPos->xnt[5] << endl;
703
704	}
705
706	_pppClient->emitNewMessage(QByteArray(strC.str().c_str()), true);
707
708	if (_opt->pppAverage == 0.0) {
709	delete newPos;
710	}
711	else {
712
713	_posAverage.push_back(newPos);
714
715	// Compute the Mean
716	// ----------------
717	ColumnVector mean(7); mean = 0.0;
718
719	QMutableVectorIterator<pppPos*> it(_posAverage);
720	while (it.hasNext()) {
721	pppPos* pp = it.next();
722	if ( (epoData->tt - pp->time) >= _opt->pppAverage ) {
723	delete pp;
724	it.remove();
725	}
726	else {
727	for (int ii = 0; ii < 7; ++ii) {
728	mean[ii] += pp->xnt[ii];
729	}
730	}
731	}
732
733	int nn = _posAverage.size();
734
735	if (nn > 0) {
736
737	mean /= nn;
738
739	// Compute the Deviation
740	// ---------------------
741	ColumnVector std(7); std = 0.0;
742	QVectorIterator<pppPos*> it2(_posAverage);
743	while (it2.hasNext()) {
744	pppPos* pp = it2.next();
745	for (int ii = 0; ii < 7; ++ii) {
746	std[ii] += (pp->xnt[ii] - mean[ii]) * (pp->xnt[ii] - mean[ii]);
747	}
748	}
749	for (int ii = 0; ii < 7; ++ii) {
750	std[ii] = sqrt(std[ii] / nn);
751	}
752
753	if (_opt->refCrdSet()) {
754	ostringstream strD; strD.setf(ios::fixed);
755	strD << _staID.data() << " AVE-XYZ "
756	<< epoData->tt.timestr(1) << " "
757	<< setw(13) << setprecision(3) << mean[0] + _opt->refCrd[0] << " +- "
758	<< setw(6) << setprecision(3) << std[0] << " "
759	<< setw(14) << setprecision(3) << mean[1] + _opt->refCrd[1] << " +- "
760	<< setw(6) << setprecision(3) << std[1] << " "
761	<< setw(14) << setprecision(3) << mean[2] + _opt->refCrd[2] << " +- "
762	<< setw(6) << setprecision(3) << std[2];
763	_pppClient->emitNewMessage(QByteArray(strD.str().c_str()), true);
764
765	ostringstream strE; strE.setf(ios::fixed);
766	strE << _staID.data() << " AVE-NEU "
767	<< epoData->tt.timestr(1) << " "
768	<< setw(13) << setprecision(3) << mean[3] << " +- "
769	<< setw(6) << setprecision(3) << std[3] << " "
770	<< setw(14) << setprecision(3) << mean[4] << " +- "
771	<< setw(6) << setprecision(3) << std[4] << " "
772	<< setw(14) << setprecision(3) << mean[5] << " +- "
773	<< setw(6) << setprecision(3) << std[5];
774	_pppClient->emitNewMessage(QByteArray(strE.str().c_str()), true);
775
776	if (_opt->estTropo) {
777	ostringstream strF; strF.setf(ios::fixed);
778	strF << _staID.data() << " AVE-TRP "
779	<< epoData->tt.timestr(1) << " "
780	<< setw(13) << setprecision(3) << mean[6] << " +- "
781	<< setw(6) << setprecision(3) << std[6] << endl;
782	_pppClient->emitNewMessage(QByteArray(strF.str().c_str()), true);
783	}
784	}
785	}
786	}
787
788	// NMEA Output
789	// -----------
790	double xyz[3];
791	xyz[0] = x();
792	xyz[1] = y();
793	xyz[2] = z();
794	double ell[3];
795	xyz2ell(xyz, ell);
796	double phiDeg = ell[0] * 180 / M_PI;
797	double lamDeg = ell[1] * 180 / M_PI;
798
799	char phiCh = 'N';
800	if (phiDeg < 0) {
801	phiDeg = -phiDeg;
802	phiCh = 'S';
803	}
804	char lamCh = 'E';
805	if (lamDeg < 0) {
806	lamDeg = -lamDeg;
807	lamCh = 'W';
808	}
809
810	string datestr = epoData->tt.datestr(0); // yyyymmdd
811	ostringstream strRMC;
812	strRMC.setf(ios::fixed);
813	strRMC << "GPRMC,"
814	<< epoData->tt.timestr(0,0) << ",A,"
815	<< setw(2) << setfill('0') << int(phiDeg)
816	<< setw(6) << setprecision(3) << setfill('0')
817	<< fmod(60*phiDeg,60) << ',' << phiCh << ','
818	<< setw(3) << setfill('0') << int(lamDeg)
819	<< setw(6) << setprecision(3) << setfill('0')
820	<< fmod(60*lamDeg,60) << ',' << lamCh << ",,,"
821	<< datestr[6] << datestr[7] << datestr[4] << datestr[5]
822	<< datestr[2] << datestr[3] << ",,";
823
824	writeNMEAstr(QString(strRMC.str().c_str()));
825
826	double dop = 2.0; // TODO
827
828	ostringstream strGGA;
829	strGGA.setf(ios::fixed);
830	strGGA << "GPGGA,"
831	<< epoData->tt.timestr(0,0) << ','
832	<< setw(2) << setfill('0') << int(phiDeg)
833	<< setw(10) << setprecision(7) << setfill('0')
834	<< fmod(60*phiDeg,60) << ',' << phiCh << ','
835	<< setw(3) << setfill('0') << int(lamDeg)
836	<< setw(10) << setprecision(7) << setfill('0')
837	<< fmod(60*lamDeg,60) << ',' << lamCh
838	<< ",1," << setw(2) << setfill('0') << epoData->sizeAll() << ','
839	<< setw(3) << setprecision(1) << dop << ','
840	<< setprecision(3) << ell[2] << ",M,0.0,M,,";
841
842	writeNMEAstr(QString(strGGA.str().c_str()));
843
844	_lastTimeOK = _time; // remember time of last successful update
845	return success;
846	}
847
848	// Outlier Detection
849	////////////////////////////////////////////////////////////////////////////
850	QString bncModel::outlierDetection(int iPhase, const ColumnVector& vv,
851	QMap<QString, t_satData*>& satData) {
852
853	Tracer tracer("bncModel::outlierDetection");
854
855	QString prnGPS;
856	QString prnGlo;
857	double maxResGPS = 0.0;
858	double maxResGlo = 0.0;
859	findMaxRes(vv, satData, prnGPS, prnGlo, maxResGPS, maxResGlo);
860
861	if (iPhase == 1) {
862	if (maxResGlo > MAXRES_PHASE_GLONASS) {
863	_log += "Outlier Phase " + prnGlo + " "
864	+ QByteArray::number(maxResGlo, 'f', 3) + "\n";
865	return prnGlo;
866	}
867	else if (maxResGPS > MAXRES_PHASE_GPS) {
868	_log += "Outlier Phase " + prnGPS + " "
869	+ QByteArray::number(maxResGPS, 'f', 3) + "\n";
870	return prnGPS;
871	}
872	}
873	else if (iPhase == 0 && maxResGPS > MAXRES_CODE) {
874	_log += "Outlier Code " + prnGPS + " "
875	+ QByteArray::number(maxResGPS, 'f', 3) + "\n";
876	return prnGPS;
877	}
878
879	return QString();
880	}
881
882	//
883	////////////////////////////////////////////////////////////////////////////
884	void bncModel::writeNMEAstr(const QString& nmStr) {
885
886	Tracer tracer("bncModel::writeNMEAstr");
887
888	unsigned char XOR = 0;
889	for (int ii = 0; ii < nmStr.length(); ii++) {
890	XOR ^= (unsigned char) nmStr[ii].toAscii();
891	}
892
893	QString outStr = '$' + nmStr
894	+ QString("*%1\n").arg(int(XOR), 0, 16).toUpper();
895
896	if (_nmeaStream) {
897	*_nmeaStream << outStr;
898	_nmeaStream->flush();
899	}
900
901	_pppClient->emitNewNMEAstr(outStr.toAscii());
902	}
903
904	//
905	//////////////////////////////////////////////////////////////////////////////
906	void bncModel::kalman(const Matrix& AA, const ColumnVector& ll,
907	const DiagonalMatrix& PP,
908	SymmetricMatrix& QQ, ColumnVector& dx) {
909
910	Tracer tracer("bncModel::kalman");
911
912	int nPar = AA.Ncols();
913	#if 1
914	int nObs = AA.Nrows();
915	UpperTriangularMatrix SS = Cholesky(QQ).t();
916
917	Matrix SA = SS*AA.t();
918	Matrix SRF(nObs+nPar, nObs+nPar); SRF = 0;
919	for (int ii = 1; ii <= nObs; ++ii) {
920	SRF(ii,ii) = 1.0 / sqrt(PP(ii,ii));
921	}
922
923	SRF.SubMatrix (nObs+1, nObs+nPar, 1, nObs) = SA;
924	SRF.SymSubMatrix(nObs+1, nObs+nPar) = SS;
925
926	UpperTriangularMatrix UU;
927	QRZ(SRF, UU);
928
929	SS = UU.SymSubMatrix(nObs+1, nObs+nPar);
930	UpperTriangularMatrix SH_rt = UU.SymSubMatrix(1, nObs);
931	Matrix YY = UU.SubMatrix(1, nObs, nObs+1, nObs+nPar);
932
933	UpperTriangularMatrix SHi = SH_rt.i();
934
935	Matrix KT = SHi * YY;
936	SymmetricMatrix Hi; Hi << SHi * SHi.t();
937
938	dx = KT.t() * ll;
939	QQ << (SS.t() * SS);
940	#else
941	DiagonalMatrix Ql = PP.i();
942	Matrix DD = QQ * AA.t();
943	SymmetricMatrix SM(nPar); SM << AA * DD + Ql;
944	UpperTriangularMatrix UU = Cholesky(SM).t();
945	UpperTriangularMatrix Ui = UU.i();
946	Matrix EE = DD * Ui;
947	Matrix KK = EE * Ui.t();
948	QQ << QQ - EE * EE.t();
949	dx = KK * ll;
950	#endif
951	}
952
953	// Phase Wind-Up Correction
954	///////////////////////////////////////////////////////////////////////////
955	double bncModel::windUp(const QString& prn, const ColumnVector& rSat,
956	const ColumnVector& rRec) {
957
958	Tracer tracer("bncModel::windUp");
959
960	double Mjd = _time.mjd() + _time.daysec() / 86400.0;
961
962	// First time - initialize to zero
963	// -------------------------------
964	if (!_windUpTime.contains(prn)) {
965	_windUpSum[prn] = 0.0;
966	}
967
968	// Compute the correction for new time
969	// -----------------------------------
970	if (!_windUpTime.contains(prn) \|\| _windUpTime[prn] != Mjd) {
971	_windUpTime[prn] = Mjd;
972
973	// Unit Vector GPS Satellite --> Receiver
974	// --------------------------------------
975	ColumnVector rho = rRec - rSat;
976	rho /= rho.norm_Frobenius();
977
978	// GPS Satellite unit Vectors sz, sy, sx
979	// -------------------------------------
980	ColumnVector sz = -rSat / rSat.norm_Frobenius();
981
982	ColumnVector xSun = Sun(Mjd);
983	xSun /= xSun.norm_Frobenius();
984
985	ColumnVector sy = crossproduct(sz, xSun);
986	ColumnVector sx = crossproduct(sy, sz);
987
988	// Effective Dipole of the GPS Satellite Antenna
989	// ---------------------------------------------
990	ColumnVector dipSat = sx - rho * DotProduct(rho,sx)
991	- crossproduct(rho, sy);
992
993	// Receiver unit Vectors rx, ry
994	// ----------------------------
995	ColumnVector rx(3);
996	ColumnVector ry(3);
997
998	double recEll[3]; xyz2ell(rRec.data(), recEll) ;
999	double neu[3];
1000
1001	neu[0] = 1.0;
1002	neu[1] = 0.0;
1003	neu[2] = 0.0;
1004	neu2xyz(recEll, neu, rx.data());
1005
1006	neu[0] = 0.0;
1007	neu[1] = -1.0;
1008	neu[2] = 0.0;
1009	neu2xyz(recEll, neu, ry.data());
1010
1011	// Effective Dipole of the Receiver Antenna
1012	// ----------------------------------------
1013	ColumnVector dipRec = rx - rho * DotProduct(rho,rx)
1014	+ crossproduct(rho, ry);
1015
1016	// Resulting Effect
1017	// ----------------
1018	double alpha = DotProduct(dipSat,dipRec) /
1019	(dipSat.norm_Frobenius() * dipRec.norm_Frobenius());
1020
1021	if (alpha > 1.0) alpha = 1.0;
1022	if (alpha < -1.0) alpha = -1.0;
1023
1024	double dphi = acos(alpha) / 2.0 / M_PI; // in cycles
1025
1026	if ( DotProduct(rho, crossproduct(dipSat, dipRec)) < 0.0 ) {
1027	dphi = -dphi;
1028	}
1029
1030	_windUpSum[prn] = floor(_windUpSum[prn] - dphi + 0.5) + dphi;
1031	}
1032
1033	return _windUpSum[prn];
1034	}
1035
1036	//
1037	///////////////////////////////////////////////////////////////////////////
1038	void bncModel::cmpEle(t_satData* satData) {
1039	Tracer tracer("bncModel::cmpEle");
1040	ColumnVector rr = satData->xx - _xcBanc.Rows(1,3);
1041	double rho = rr.norm_Frobenius();
1042
1043	double neu[3];
1044	xyz2neu(_ellBanc.data(), rr.data(), neu);
1045
1046	satData->eleSat = acos( sqrt(neu[0]neu[0] + neu[1]neu[1]) / rho );
1047	if (neu[2] < 0) {
1048	satData->eleSat *= -1.0;
1049	}
1050	satData->azSat = atan2(neu[1], neu[0]);
1051	}
1052
1053	//
1054	///////////////////////////////////////////////////////////////////////////
1055	void bncModel::addAmb(t_satData* satData) {
1056	Tracer tracer("bncModel::addAmb");
1057	bool found = false;
1058	for (int iPar = 1; iPar <= _params.size(); iPar++) {
1059	if (_params[iPar-1]->type == bncParam::AMB_L3 &&
1060	_params[iPar-1]->prn == satData->prn) {
1061	found = true;
1062	break;
1063	}
1064	}
1065	if (!found) {
1066	bncParam* par = new bncParam(bncParam::AMB_L3,
1067	_params.size()+1, satData->prn);
1068	_params.push_back(par);
1069	par->xx = satData->L3 - cmpValue(satData, true);
1070	}
1071	}
1072
1073	//
1074	///////////////////////////////////////////////////////////////////////////
1075	void bncModel::addObs(int iPhase, unsigned& iObs, t_satData* satData,
1076	Matrix& AA, ColumnVector& ll, DiagonalMatrix& PP) {
1077
1078	Tracer tracer("bncModel::addObs");
1079
1080	const double ELEWGHT = 20.0;
1081	double ellWgtCoef = 1.0;
1082	double eleD = satData->eleSat * 180.0 / M_PI;
1083	if (eleD < ELEWGHT) {
1084	ellWgtCoef = 1.5 - 0.5 / (ELEWGHT - 10.0) * (eleD - 10.0);
1085	}
1086
1087	// Remember Observation Index
1088	// --------------------------
1089	++iObs;
1090	satData->obsIndex = iObs;
1091
1092	// Phase Observations
1093	// ------------------
1094	if (iPhase == 1) {
1095	ll(iObs) = satData->L3 - cmpValue(satData, true);
1096	double sigL3 = _opt->sigL3;
1097	if (satData->system() == 'R') {
1098	sigL3 *= GLONASS_WEIGHT_FACTOR;
1099	}
1100	PP(iObs,iObs) = 1.0 / (sigL3 * sigL3) / (ellWgtCoef * ellWgtCoef);
1101	for (int iPar = 1; iPar <= _params.size(); iPar++) {
1102	if (_params[iPar-1]->type == bncParam::AMB_L3 &&
1103	_params[iPar-1]->prn == satData->prn) {
1104	ll(iObs) -= _params[iPar-1]->xx;
1105	}
1106	AA(iObs, iPar) = _params[iPar-1]->partial(satData, true);
1107	}
1108	}
1109
1110	// Code Observations
1111	// -----------------
1112	else {
1113	ll(iObs) = satData->P3 - cmpValue(satData, false);
1114	PP(iObs,iObs) = 1.0 / (_opt->sigP3 * _opt->sigP3) / (ellWgtCoef * ellWgtCoef);
1115	for (int iPar = 1; iPar <= _params.size(); iPar++) {
1116	AA(iObs, iPar) = _params[iPar-1]->partial(satData, false);
1117	}
1118	}
1119	}
1120
1121	//
1122	///////////////////////////////////////////////////////////////////////////
1123	QByteArray bncModel::printRes(int iPhase, const ColumnVector& vv,
1124	const QMap<QString, t_satData*>& satDataMap) {
1125
1126	Tracer tracer("bncModel::printRes");
1127
1128	ostringstream str;
1129	str.setf(ios::fixed);
1130
1131	QMapIterator<QString, t_satData*> it(satDataMap);
1132	while (it.hasNext()) {
1133	it.next();
1134	t_satData* satData = it.value();
1135	if (satData->obsIndex != 0) {
1136	str << _time.timestr(1)
1137	<< " RES " << satData->prn.toAscii().data()
1138	<< (iPhase ? " L3 " : " P3 ")
1139	<< setw(9) << setprecision(4) << vv(satData->obsIndex) << endl;
1140	}
1141	}
1142
1143	return QByteArray(str.str().c_str());
1144	}
1145
1146	//
1147	///////////////////////////////////////////////////////////////////////////
1148	void bncModel::findMaxRes(const ColumnVector& vv,
1149	const QMap<QString, t_satData*>& satData,
1150	QString& prnGPS, QString& prnGlo,
1151	double& maxResGPS, double& maxResGlo) {
1152
1153	Tracer tracer("bncModel::findMaxRes");
1154
1155	maxResGPS = 0.0;
1156	maxResGlo = 0.0;
1157
1158	QMapIterator<QString, t_satData*> it(satData);
1159	while (it.hasNext()) {
1160	it.next();
1161	t_satData* satData = it.value();
1162	if (satData->obsIndex != 0) {
1163	QString prn = satData->prn;
1164	if (prn[0] == 'R') {
1165	if (fabs(vv(satData->obsIndex)) > maxResGlo) {
1166	maxResGlo = fabs(vv(satData->obsIndex));
1167	prnGlo = prn;
1168	}
1169	}
1170	else {
1171	if (fabs(vv(satData->obsIndex)) > maxResGPS) {
1172	maxResGPS = fabs(vv(satData->obsIndex));
1173	prnGPS = prn;
1174	}
1175	}
1176	}
1177	}
1178	}
1179
1180	// Update Step (private - loop over outliers)
1181	////////////////////////////////////////////////////////////////////////////
1182	t_irc bncModel::update_p(t_epoData* epoData) {
1183
1184	Tracer tracer("bncModel::update_p");
1185
1186	// Save Variance-Covariance Matrix, and Status Vector
1187	// --------------------------------------------------
1188	rememberState(epoData);
1189
1190	QString lastOutlierPrn;
1191
1192	// Try with all satellites, then with all minus one, etc.
1193	// ------------------------------------------------------
1194	while (selectSatellites(lastOutlierPrn, epoData->satData) == success) {
1195
1196	QByteArray strResCode;
1197	QByteArray strResPhase;
1198
1199	// Bancroft Solution
1200	// -----------------
1201	if (cmpBancroft(epoData) != success) {
1202	break;
1203	}
1204
1205	// First update using code observations, then phase observations
1206	// -------------------------------------------------------------
1207	for (int iPhase = 0; iPhase <= (_opt->usePhase ? 1 : 0); iPhase++) {
1208
1209	// Status Prediction
1210	// -----------------
1211	predict(iPhase, epoData);
1212
1213	// Create First-Design Matrix
1214	// --------------------------
1215	unsigned nPar = _params.size();
1216	unsigned nObs = 0;
1217	if (iPhase == 0) {
1218	nObs = epoData->sizeAll() - epoData->sizeSys('R'); // Glonass code not used
1219	}
1220	else {
1221	nObs = epoData->sizeAll();
1222	}
1223
1224	// Prepare first-design Matrix, vector observed-computed
1225	// -----------------------------------------------------
1226	Matrix AA(nObs, nPar); // first design matrix
1227	ColumnVector ll(nObs); // tems observed-computed
1228	DiagonalMatrix PP(nObs); PP = 0.0;
1229
1230	unsigned iObs = 0;
1231	QMapIterator<QString, t_satData*> it(epoData->satData);
1232	while (it.hasNext()) {
1233	it.next();
1234	t_satData* satData = it.value();
1235	if (iPhase == 1 \|\| satData->system() != 'R') {
1236	QString prn = satData->prn;
1237	addObs(iPhase, iObs, satData, AA, ll, PP);
1238	}
1239	}
1240
1241	// Compute Filter Update
1242	// ---------------------
1243	ColumnVector dx;
1244	kalman(AA, ll, PP, _QQ, dx);
1245	ColumnVector vv = ll - AA * dx;
1246
1247	// Print Residuals
1248	// ---------------
1249	if (iPhase == 0) {
1250	strResCode = printRes(iPhase, vv, epoData->satData);
1251	}
1252	else {
1253	strResPhase = printRes(iPhase, vv, epoData->satData);
1254	}
1255
1256	// Check the residuals
1257	// -------------------
1258	lastOutlierPrn = outlierDetection(iPhase, vv, epoData->satData);
1259
1260	// No Outlier Detected
1261	// -------------------
1262	if (lastOutlierPrn.isEmpty()) {
1263
1264	QVectorIterator<bncParam*> itPar(_params);
1265	while (itPar.hasNext()) {
1266	bncParam* par = itPar.next();
1267	par->xx += dx(par->index);
1268	}
1269
1270	if (!_opt->usePhase \|\| iPhase == 1) {
1271	if (_outlierGPS.size() > 0 \|\| _outlierGlo.size() > 0) {
1272	_log += "Neglected PRNs: ";
1273	if (!_outlierGPS.isEmpty()) {
1274	_log += _outlierGPS.last() + ' ';
1275	}
1276	QStringListIterator itGlo(_outlierGlo);
1277	while (itGlo.hasNext()) {
1278	QString prn = itGlo.next();
1279	_log += prn + ' ';
1280	}
1281	}
1282	_log += '\n';
1283
1284	_log += strResCode + strResPhase;
1285
1286	return success;
1287	}
1288	}
1289
1290	// Outlier Found
1291	// -------------
1292	else {
1293	restoreState(epoData);
1294	break;
1295	}
1296
1297	} // for iPhase
1298
1299	} // while selectSatellites
1300
1301	restoreState(epoData);
1302	return failure;
1303	}
1304
1305	// Remeber Original State Vector and Variance-Covariance Matrix
1306	////////////////////////////////////////////////////////////////////////////
1307	void bncModel::rememberState(t_epoData* epoData) {
1308
1309	_QQ_sav = _QQ;
1310
1311	QVectorIterator<bncParam*> itSav(_params_sav);
1312	while (itSav.hasNext()) {
1313	bncParam* par = itSav.next();
1314	delete par;
1315	}
1316	_params_sav.clear();
1317
1318	QVectorIterator<bncParam*> it(_params);
1319	while (it.hasNext()) {
1320	bncParam* par = it.next();
1321	_params_sav.push_back(new bncParam(*par));
1322	}
1323
1324	_epoData_sav->deepCopy(epoData);
1325	}
1326
1327	// Restore Original State Vector and Variance-Covariance Matrix
1328	////////////////////////////////////////////////////////////////////////////
1329	void bncModel::restoreState(t_epoData* epoData) {
1330
1331	_QQ = _QQ_sav;
1332
1333	QVectorIterator<bncParam*> it(_params);
1334	while (it.hasNext()) {
1335	bncParam* par = it.next();
1336	delete par;
1337	}
1338	_params.clear();
1339
1340	QVectorIterator<bncParam*> itSav(_params_sav);
1341	while (itSav.hasNext()) {
1342	bncParam* par = itSav.next();
1343	_params.push_back(new bncParam(*par));
1344	}
1345
1346	epoData->deepCopy(_epoData_sav);
1347	}
1348
1349	//
1350	////////////////////////////////////////////////////////////////////////////
1351	t_irc bncModel::selectSatellites(const QString& lastOutlierPrn,
1352	QMap<QString, t_satData*>& satData) {
1353
1354	// First Call
1355	// ----------
1356	if (lastOutlierPrn.isEmpty()) {
1357	_outlierGPS.clear();
1358	_outlierGlo.clear();
1359	return success;
1360	}
1361
1362	// Second and next trials
1363	// ----------------------
1364	else {
1365
1366	if (lastOutlierPrn[0] == 'R') {
1367	_outlierGlo << lastOutlierPrn;
1368	}
1369
1370	// Remove all Glonass Outliers
1371	// ---------------------------
1372	QStringListIterator it(_outlierGlo);
1373	while (it.hasNext()) {
1374	QString prn = it.next();
1375	if (satData.contains(prn)) {
1376	delete satData.take(prn);
1377	}
1378	}
1379
1380	if (lastOutlierPrn[0] == 'R') {
1381	_outlierGPS.clear();
1382	return success;
1383	}
1384
1385	// GPS Outlier appeared for the first time - try to delete it
1386	// ----------------------------------------------------------
1387	if (_outlierGPS.indexOf(lastOutlierPrn) == -1) {
1388	_outlierGPS << lastOutlierPrn;
1389	if (satData.contains(lastOutlierPrn)) {
1390	delete satData.take(lastOutlierPrn);
1391	}
1392	return success;
1393	}
1394
1395	}
1396
1397	return failure;
1398	}

Note: See TracBrowser for help on using the repository browser.

Download in other formats: