Context Navigation

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

Last change on this file since 3711 was 3639, checked in by mervart, 13 years ago

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

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

Download in other formats: