Context Navigation

source: ntrip/trunk/BNC/src/combination/bnccomb.cpp@ 8938

Last change on this file since 8938 was 8901, checked in by stuerze, 5 years ago
upgrade to newmat11 library
File size: 35.0 KB

Line
1	/* -------------------------------------------------------------------------
2	* BKG NTRIP Client
3	* -------------------------------------------------------------------------
4	*
5	* Class: bncComb
6	*
7	* Purpose: Combinations of Orbit/Clock Corrections
8	*
9	* Author: L. Mervart
10	*
11	* Created: 22-Jan-2011
12	*
13	* Changes:
14	*
15	* -----------------------------------------------------------------------*/
16
17	#include <newmatio.h>
18	#include <iomanip>
19	#include <sstream>
20
21	#include "bnccomb.h"
22	#include "bnccore.h"
23	#include "upload/bncrtnetdecoder.h"
24	#include "bncsettings.h"
25	#include "bncutils.h"
26	#include "bncsp3.h"
27	#include "bncantex.h"
28	#include "t_prn.h"
29
30	const double sig0_offAC = 1000.0;
31	const double sig0_offACSat = 100.0;
32	const double sigP_offACSat = 0.01;
33	const double sig0_clkSat = 100.0;
34
35	const double sigObs = 0.05;
36
37	using namespace std;
38
39	// Constructor
40	////////////////////////////////////////////////////////////////////////////
41	bncComb::cmbParam::cmbParam(parType type_, int index_, const QString& ac_, const QString& prn_) {
42
43	type = type_;
44	index = index_;
45	AC = ac_;
46	prn = prn_;
47	xx = 0.0;
48	eph = 0;
49
50	if (type == offACgps) {
51	epoSpec = true;
52	sig0 = sig0_offAC;
53	sigP = sig0;
54	}
55	else if (type == offACglo) {
56	epoSpec = true;
57	sig0 = sig0_offAC;
58	sigP = sig0;
59	}
60	else if (type == offACSat) {
61	epoSpec = false;
62	sig0 = sig0_offACSat;
63	sigP = sigP_offACSat;
64	}
65	else if (type == clkSat) {
66	epoSpec = true;
67	sig0 = sig0_clkSat;
68	sigP = sig0;
69	}
70	}
71
72	// Destructor
73	////////////////////////////////////////////////////////////////////////////
74	bncComb::cmbParam::~cmbParam() {
75	}
76
77	// Partial
78	////////////////////////////////////////////////////////////////////////////
79	double bncComb::cmbParam::partial(const QString& AC_, const QString& prn_) {
80
81	if (type == offACgps) {
82	if (AC == AC_ && prn_[0] == 'G') {
83	return 1.0;
84	}
85	}
86	else if (type == offACglo) {
87	if (AC == AC_ && prn_[0] == 'R') {
88	return 1.0;
89	}
90	}
91	else if (type == offACSat) {
92	if (AC == AC_ && prn == prn_) {
93	return 1.0;
94	}
95	}
96	else if (type == clkSat) {
97	if (prn == prn_) {
98	return 1.0;
99	}
100	}
101
102	return 0.0;
103	}
104
105	//
106	////////////////////////////////////////////////////////////////////////////
107	QString bncComb::cmbParam::toString() const {
108
109	QString outStr;
110
111	if (type == offACgps) {
112	outStr = "AC offset GPS " + AC;
113	}
114	else if (type == offACglo) {
115	outStr = "AC offset GLO " + AC;
116	}
117	else if (type == offACSat) {
118	outStr = "Sat Offset " + AC + " " + prn.mid(0,3);
119	}
120	else if (type == clkSat) {
121	outStr = "Clk Corr " + prn.mid(0,3);
122	}
123
124	return outStr;
125	}
126
127	// Singleton
128	////////////////////////////////////////////////////////////////////////////
129	bncComb* bncComb::instance() {
130	static bncComb _bncComb;
131	return &_bncComb;
132	}
133
134	// Constructor
135	////////////////////////////////////////////////////////////////////////////
136	bncComb::bncComb() : _ephUser(true) {
137
138	bncSettings settings;
139
140	QStringList cmbStreams = settings.value("cmbStreams").toStringList();
141
142	_cmbSampl = settings.value("cmbSampl").toInt();
143	if (_cmbSampl <= 0) {
144	_cmbSampl = 10;
145	}
146
147	_masterMissingEpochs = 0;
148
149	if (cmbStreams.size() >= 1 && !cmbStreams[0].isEmpty()) {
150	QListIterator<QString> it(cmbStreams);
151	while (it.hasNext()) {
152	QStringList hlp = it.next().split(" ");
153	cmbAC* newAC = new cmbAC();
154	newAC->mountPoint = hlp[0];
155	newAC->name = hlp[1];
156	newAC->weight = hlp[2].toDouble();
157	if (_masterOrbitAC.isEmpty()) {
158	_masterOrbitAC = newAC->name;
159	}
160	_ACs.append(newAC);
161	}
162	}
163
164	_rtnetDecoder = 0;
165
166	connect(this, SIGNAL(newMessage(QByteArray,bool)),
167	BNC_CORE, SLOT(slotMessage(const QByteArray,bool)));
168
169	connect(BNC_CORE, SIGNAL(providerIDChanged(QString)),
170	this, SLOT(slotProviderIDChanged(QString)));
171
172	connect(BNC_CORE, SIGNAL(newOrbCorrections(QList<t_orbCorr>)),
173	this, SLOT(slotNewOrbCorrections(QList<t_orbCorr>)));
174
175	connect(BNC_CORE, SIGNAL(newClkCorrections(QList<t_clkCorr>)),
176	this, SLOT(slotNewClkCorrections(QList<t_clkCorr>)));
177
178	// Combination Method
179	// ------------------
180	if (settings.value("cmbMethod").toString() == "Single-Epoch") {
181	_method = singleEpoch;
182	}
183	else {
184	_method = filter;
185	}
186
187	// Use Glonass
188	// -----------
189	if ( Qt::CheckState(settings.value("cmbUseGlonass").toInt()) == Qt::Checked) {
190	_useGlonass = true;
191	}
192	else {
193	_useGlonass = false;
194	}
195
196	// Initialize Parameters (model: Clk_Corr = AC_Offset + Sat_Offset + Clk)
197	// ----------------------------------------------------------------------
198	if (_method == filter) {
199	int nextPar = 0;
200	QListIterator<cmbAC*> it(_ACs);
201	while (it.hasNext()) {
202	cmbAC* AC = it.next();
203	_params.push_back(new cmbParam(cmbParam::offACgps, ++nextPar, AC->name, ""));
204	for (unsigned iGps = 1; iGps <= t_prn::MAXPRN_GPS; iGps++) {
205	QString prn = QString("G%1_0").arg(iGps, 2, 10, QChar('0'));
206	_params.push_back(new cmbParam(cmbParam::offACSat, ++nextPar,
207	AC->name, prn));
208	}
209	if (_useGlonass) {
210	_params.push_back(new cmbParam(cmbParam::offACglo, ++nextPar, AC->name, ""));
211	for (unsigned iGlo = 1; iGlo <= t_prn::MAXPRN_GLONASS; iGlo++) {
212	QString prn = QString("R%1_0").arg(iGlo, 2, 10, QChar('0'));
213	_params.push_back(new cmbParam(cmbParam::offACSat, ++nextPar,
214	AC->name, prn));
215	}
216	}
217	}
218	for (unsigned iGps = 1; iGps <= t_prn::MAXPRN_GPS; iGps++) {
219	QString prn = QString("G%1_0").arg(iGps, 2, 10, QChar('0'));
220	_params.push_back(new cmbParam(cmbParam::clkSat, ++nextPar, "", prn));
221	}
222	if (_useGlonass) {
223	for (unsigned iGlo = 1; iGlo <= t_prn::MAXPRN_GLONASS; iGlo++) {
224	QString prn = QString("R%1_0").arg(iGlo, 2, 10, QChar('0'));
225	_params.push_back(new cmbParam(cmbParam::clkSat, ++nextPar, "", prn));
226	}
227	}
228
229	// Initialize Variance-Covariance Matrix
230	// -------------------------------------
231	_QQ.ReSize(_params.size());
232	_QQ = 0.0;
233	for (int iPar = 1; iPar <= _params.size(); iPar++) {
234	cmbParam* pp = _params[iPar-1];
235	_QQ(iPar,iPar) = pp->sig0 * pp->sig0;
236	}
237	}
238
239	// ANTEX File
240	// ----------
241	_antex = 0;
242	QString antexFileName = settings.value("uploadAntexFile").toString();
243	if (!antexFileName.isEmpty()) {
244	_antex = new bncAntex();
245	if (_antex->readFile(antexFileName) != success) {
246	emit newMessage("wrong ANTEX file", true);
247	delete _antex;
248	_antex = 0;
249	}
250	}
251
252	// Maximal Residuum
253	// ----------------
254	_MAXRES = settings.value("cmbMaxres").toDouble();
255	if (_MAXRES <= 0.0) {
256	_MAXRES = 999.0;
257	}
258	}
259
260	// Destructor
261	////////////////////////////////////////////////////////////////////////////
262	bncComb::~bncComb() {
263	QListIterator<cmbAC*> icAC(_ACs);
264	while (icAC.hasNext()) {
265	delete icAC.next();
266	}
267	delete _rtnetDecoder;
268	delete _antex;
269	for (int iPar = 1; iPar <= _params.size(); iPar++) {
270	delete _params[iPar-1];
271	}
272	QListIterator<bncTime> itTime(_buffer.keys());
273	while (itTime.hasNext()) {
274	bncTime epoTime = itTime.next();
275	_buffer.remove(epoTime);
276	}
277	}
278
279	// Remember orbit corrections
280	////////////////////////////////////////////////////////////////////////////
281	void bncComb::slotNewOrbCorrections(QList<t_orbCorr> orbCorrections) {
282	QMutexLocker locker(&_mutex);
283
284	for (int ii = 0; ii < orbCorrections.size(); ii++) {
285	t_orbCorr& orbCorr = orbCorrections[ii];
286	QString staID(orbCorr._staID.c_str());
287	QString prn(orbCorr._prn.toInternalString().c_str());
288
289	// Find/Check the AC Name
290	// ----------------------
291	QString acName;
292	QListIterator<cmbAC*> icAC(_ACs);
293	while (icAC.hasNext()) {
294	cmbAC* AC = icAC.next();
295	if (AC->mountPoint == staID) {
296	acName = AC->name;
297	break;
298	}
299	}
300	if (acName.isEmpty()) {
301	continue;
302	}
303
304	// Store the correction
305	// --------------------
306	QMap<t_prn, t_orbCorr>& storage = _orbCorrections[acName];
307	storage[orbCorr._prn] = orbCorr;
308	}
309	}
310
311	// Process clock corrections
312	////////////////////////////////////////////////////////////////////////////
313	void bncComb::slotNewClkCorrections(QList<t_clkCorr> clkCorrections) {
314	QMutexLocker locker(&_mutex);
315
316	bncTime lastTime;
317
318	for (int ii = 0; ii < clkCorrections.size(); ii++) {
319	t_clkCorr& clkCorr = clkCorrections[ii];
320	QString staID(clkCorr._staID.c_str());
321	QString prn(clkCorr._prn.toInternalString().c_str());
322
323	// Set the last time
324	// -----------------
325	if (lastTime.undef() \|\| clkCorr._time > lastTime) {
326	lastTime = clkCorr._time;
327	}
328
329	// Find/Check the AC Name
330	// ----------------------
331	QString acName;
332	QListIterator<cmbAC*> icAC(_ACs);
333	while (icAC.hasNext()) {
334	cmbAC* AC = icAC.next();
335	if (AC->mountPoint == staID) {
336	acName = AC->name;
337	break;
338	}
339	}
340	if (acName.isEmpty()) {
341	continue;
342	}
343
344	// Check GLONASS
345	// -------------
346	if (!_useGlonass && clkCorr._prn.system() == 'R') {
347	continue;
348	}
349
350	if (
351	clkCorr._prn.system() == 'E' \|\|
352	clkCorr._prn.system() == 'C' \|\|
353	clkCorr._prn.system() == 'J' \|\|
354	clkCorr._prn.system() == 'I' \|\|
355	clkCorr._prn.system() == 'S' ) {
356	continue;
357	}
358
359	// Check Modulo Time
360	// -----------------
361	int sec = int(nint(clkCorr._time.gpssec()*10));
362	if (sec % (_cmbSampl*10) != 0.0) {
363	continue;
364	}
365
366	// Check Correction Age
367	// --------------------
368	if (_resTime.valid() && clkCorr._time <= _resTime) {
369	emit newMessage("bncComb: old correction: " + acName.toLatin1() + " " + prn.mid(0,3).toLatin1(), true);
370	continue;
371	}
372
373	// Create new correction
374	// ---------------------
375	cmbCorr* newCorr = new cmbCorr();
376	newCorr->_prn = prn;
377	newCorr->_time = clkCorr._time;
378	newCorr->_iod = clkCorr._iod;
379	newCorr->_acName = acName;
380	newCorr->_clkCorr = clkCorr;
381
382	// Check orbit correction
383	// ----------------------
384	if (!_orbCorrections.contains(acName)) {
385	delete newCorr;
386	continue;
387	}
388	else {
389	QMap<t_prn, t_orbCorr>& storage = _orbCorrections[acName];
390	if (!storage.contains(clkCorr._prn) \|\| storage[clkCorr._prn]._iod != newCorr->_iod) {
391	delete newCorr;
392	continue;
393	}
394	else {
395	newCorr->_orbCorr = storage[clkCorr._prn];
396	}
397	}
398
399	// Check the Ephemeris
400	//--------------------
401	t_eph* ephLast = _ephUser.ephLast(prn);
402	t_eph* ephPrev = _ephUser.ephPrev(prn);
403	if (ephLast == 0) {
404	emit newMessage("bncComb: eph not found for " + prn.mid(0,3).toLatin1(), true);
405	delete newCorr;
406	continue;
407	}
408	else {
409	if (ephLast->IOD() == newCorr->_iod) {
410	newCorr->_eph = ephLast;
411	}
412	else if (ephPrev && ephPrev->IOD() == newCorr->_iod) {
413	newCorr->_eph = ephPrev;
414	switchToLastEph(ephLast, newCorr);
415	}
416	else {
417	emit newMessage("bncComb: eph not found for " + prn.mid(0,3).toLatin1() +
418	QString(" with IOD %1").arg(newCorr->_iod).toLatin1(), true);
419	delete newCorr;
420	continue;
421	}
422	}
423
424	// Store correction into the buffer
425	// --------------------------------
426	QVector<cmbCorr*>& corrs = _buffer[newCorr->_time].corrs;
427	corrs.push_back(newCorr);
428	}
429
430	// Process previous Epoch(s)
431	// -------------------------
432	const double outWait = 1.0 * _cmbSampl;
433	QListIterator<bncTime> itTime(_buffer.keys());
434	while (itTime.hasNext()) {
435	bncTime epoTime = itTime.next();
436	if (epoTime < lastTime - outWait) {
437	_resTime = epoTime;
438	processEpoch();
439	}
440	}
441	}
442
443	// Change the correction so that it refers to last received ephemeris
444	////////////////////////////////////////////////////////////////////////////
445	void bncComb::switchToLastEph(t_eph* lastEph, cmbCorr* corr) {
446
447	if (corr->_eph == lastEph) {
448	return;
449	}
450
451	ColumnVector oldXC(6);
452	ColumnVector oldVV(3);
453	corr->_eph->getCrd(corr->_time, oldXC, oldVV, false);
454
455	ColumnVector newXC(6);
456	ColumnVector newVV(3);
457	lastEph->getCrd(corr->_time, newXC, newVV, false);
458
459	ColumnVector dX = newXC.Rows(1,3) - oldXC.Rows(1,3);
460	ColumnVector dV = newVV - oldVV;
461	double dC = newXC(4) - oldXC(4);
462
463	ColumnVector dRAO(3);
464	XYZ_to_RSW(newXC.Rows(1,3), newVV, dX, dRAO);
465
466	ColumnVector dDotRAO(3);
467	XYZ_to_RSW(newXC.Rows(1,3), newVV, dV, dDotRAO);
468
469	QString msg = "switch corr " + corr->_prn.mid(0,3)
470	+ QString(" %1 -> %2 %3").arg(corr->_iod,3).arg(lastEph->IOD(),3).arg(dC*t_CST::c, 8, 'f', 4);
471
472	emit newMessage(msg.toLatin1(), false);
473
474	corr->_iod = lastEph->IOD();
475	corr->_eph = lastEph;
476
477	corr->_orbCorr._xr += dRAO;
478	corr->_orbCorr._dotXr += dDotRAO;
479	corr->_clkCorr._dClk -= dC;
480	}
481
482	// Process Epoch
483	////////////////////////////////////////////////////////////////////////////
484	void bncComb::processEpoch() {
485
486	_log.clear();
487
488	QTextStream out(&_log, QIODevice::WriteOnly);
489
490	out << endl << "Combination:" << endl
491	<< "------------------------------" << endl;
492
493	// Observation Statistics
494	// ----------------------
495	bool masterPresent = false;
496	QListIterator<cmbAC*> icAC(_ACs);
497	while (icAC.hasNext()) {
498	cmbAC* AC = icAC.next();
499	AC->numObs = 0;
500	QVectorIterator<cmbCorr*> itCorr(corrs());
501	while (itCorr.hasNext()) {
502	cmbCorr* corr = itCorr.next();
503	if (corr->_acName == AC->name) {
504	AC->numObs += 1;
505	if (AC->name == _masterOrbitAC) {
506	masterPresent = true;
507	}
508	}
509	}
510	out << AC->name.toLatin1().data() << ": " << AC->numObs << endl;
511	}
512
513	// If Master not present, switch to another one
514	// --------------------------------------------
515	const unsigned switchMasterAfterGap = 1;
516	if (masterPresent) {
517	_masterMissingEpochs = 0;
518	}
519	else {
520	++_masterMissingEpochs;
521	if (_masterMissingEpochs < switchMasterAfterGap) {
522	out << "Missing Master, Epoch skipped" << endl;
523	_buffer.remove(_resTime);
524	emit newMessage(_log, false);
525	return;
526	}
527	else {
528	_masterMissingEpochs = 0;
529	QListIterator<cmbAC*> icAC(_ACs);
530	while (icAC.hasNext()) {
531	cmbAC* AC = icAC.next();
532	if (AC->numObs > 0) {
533	out << "Switching Master AC "
534	<< _masterOrbitAC.toLatin1().data() << " --> "
535	<< AC->name.toLatin1().data() << " "
536	<< _resTime.datestr().c_str() << " "
537	<< _resTime.timestr().c_str() << endl;
538	_masterOrbitAC = AC->name;
539	break;
540	}
541	}
542	}
543	}
544
545	QMap<QString, cmbCorr*> resCorr;
546
547	// Perform the actual Combination using selected Method
548	// ----------------------------------------------------
549	t_irc irc;
550	ColumnVector dx;
551	if (_method == filter) {
552	irc = processEpoch_filter(out, resCorr, dx);
553	}
554	else {
555	irc = processEpoch_singleEpoch(out, resCorr, dx);
556	}
557
558	// Update Parameter Values, Print Results
559	// --------------------------------------
560	if (irc == success) {
561	for (int iPar = 1; iPar <= _params.size(); iPar++) {
562	cmbParam* pp = _params[iPar-1];
563	pp->xx += dx(iPar);
564	if (pp->type == cmbParam::clkSat) {
565	if (resCorr.find(pp->prn) != resCorr.end()) {
566	resCorr[pp->prn]->_dClkResult = pp->xx / t_CST::c;
567	}
568	}
569	out << _resTime.datestr().c_str() << " "
570	<< _resTime.timestr().c_str() << " ";
571	out.setRealNumberNotation(QTextStream::FixedNotation);
572	out.setFieldWidth(8);
573	out.setRealNumberPrecision(4);
574	out << pp->toString() << " "
575	<< pp->xx << " +- " << sqrt(_QQ(pp->index,pp->index)) << endl;
576	out.setFieldWidth(0);
577	}
578	printResults(out, resCorr);
579	dumpResults(resCorr);
580	}
581
582	// Delete Data, emit Message
583	// -------------------------
584	_buffer.remove(_resTime);
585	emit newMessage(_log, false);
586	}
587
588	// Process Epoch - Filter Method
589	////////////////////////////////////////////////////////////////////////////
590	t_irc bncComb::processEpoch_filter(QTextStream& out,
591	QMap<QString, cmbCorr*>& resCorr,
592	ColumnVector& dx) {
593
594	// Prediction Step
595	// ---------------
596	int nPar = _params.size();
597	ColumnVector x0(nPar);
598	for (int iPar = 1; iPar <= _params.size(); iPar++) {
599	cmbParam* pp = _params[iPar-1];
600	if (pp->epoSpec) {
601	pp->xx = 0.0;
602	_QQ.Row(iPar) = 0.0;
603	_QQ.Column(iPar) = 0.0;
604	_QQ(iPar,iPar) = pp->sig0 * pp->sig0;
605	}
606	else {
607	_QQ(iPar,iPar) += pp->sigP * pp->sigP;
608	}
609	x0(iPar) = pp->xx;
610	}
611
612	// Check Satellite Positions for Outliers
613	// --------------------------------------
614	if (checkOrbits(out) != success) {
615	return failure;
616	}
617
618	// Update and outlier detection loop
619	// ---------------------------------
620	SymmetricMatrix QQ_sav = _QQ;
621	while (true) {
622
623	Matrix AA;
624	ColumnVector ll;
625	DiagonalMatrix PP;
626
627	if (createAmat(AA, ll, PP, x0, resCorr) != success) {
628	return failure;
629	}
630
631	dx.ReSize(nPar); dx = 0.0;
632	kalman(AA, ll, PP, _QQ, dx);
633
634	ColumnVector vv = ll - AA * dx;
635
636	int maxResIndex;
637	double maxRes = vv.maximum_absolute_value1(maxResIndex);
638	out.setRealNumberNotation(QTextStream::FixedNotation);
639	out.setRealNumberPrecision(3);
640	out << _resTime.datestr().c_str() << " " << _resTime.timestr().c_str()
641	<< " Maximum Residuum " << maxRes << ' '
642	<< corrs()[maxResIndex-1]->_acName << ' ' << corrs()[maxResIndex-1]->_prn.mid(0,3);
643	if (maxRes > _MAXRES) {
644	for (int iPar = 1; iPar <= _params.size(); iPar++) {
645	cmbParam* pp = _params[iPar-1];
646	if (pp->type == cmbParam::offACSat &&
647	pp->AC == corrs()[maxResIndex-1]->_acName &&
648	pp->prn == corrs()[maxResIndex-1]->_prn.mid(0,3)) {
649	QQ_sav.Row(iPar) = 0.0;
650	QQ_sav.Column(iPar) = 0.0;
651	QQ_sav(iPar,iPar) = pp->sig0 * pp->sig0;
652	}
653	}
654
655	out << " Outlier" << endl;
656	_QQ = QQ_sav;
657	corrs().remove(maxResIndex-1);
658	}
659	else {
660	out << " OK" << endl;
661	out.setRealNumberNotation(QTextStream::FixedNotation);
662	out.setRealNumberPrecision(4);
663	for (int ii = 0; ii < corrs().size(); ii++) {
664	const cmbCorr* corr = corrs()[ii];
665	out << _resTime.datestr().c_str() << ' '
666	<< _resTime.timestr().c_str() << " "
667	<< corr->_acName << ' ' << corr->_prn.mid(0,3);
668	out.setFieldWidth(10);
669	out << " res = " << vv[ii] << endl;
670	out.setFieldWidth(0);
671	}
672	break;
673	}
674	}
675
676	return success;
677	}
678
679	// Print results
680	////////////////////////////////////////////////////////////////////////////
681	void bncComb::printResults(QTextStream& out,
682	const QMap<QString, cmbCorr*>& resCorr) {
683
684	QMapIterator<QString, cmbCorr*> it(resCorr);
685	while (it.hasNext()) {
686	it.next();
687	cmbCorr* corr = it.value();
688	const t_eph* eph = corr->_eph;
689	if (eph) {
690	ColumnVector xc(6);
691	ColumnVector vv(3);
692	eph->getCrd(_resTime, xc, vv, false);
693
694	out << _resTime.datestr().c_str() << " "
695	<< _resTime.timestr().c_str() << " ";
696	out.setFieldWidth(3);
697	out << "Full Clock " << corr->_prn.mid(0,3) << " " << corr->_iod << " ";
698	out.setFieldWidth(14);
699	out << (xc(4) + corr->_dClkResult) * t_CST::c << endl;
700	out.setFieldWidth(0);
701	}
702	else {
703	out << "bncComb::printResuls bug" << endl;
704	}
705	}
706	}
707
708	// Send results to RTNet Decoder and directly to PPP Client
709	////////////////////////////////////////////////////////////////////////////
710	void bncComb::dumpResults(const QMap<QString, cmbCorr*>& resCorr) {
711
712	QList<t_orbCorr> orbCorrections;
713	QList<t_clkCorr> clkCorrections;
714
715	QString outLines;
716	QStringList corrLines;
717
718	unsigned year, month, day, hour, minute;
719	double sec;
720	_resTime.civil_date(year, month, day);
721	_resTime.civil_time(hour, minute, sec);
722
723	outLines.sprintf("* %4d %2d %2d %d %d %12.8f\n",
724	year, month, day, hour, minute, sec);
725
726	QMapIterator<QString, cmbCorr*> it(resCorr);
727	while (it.hasNext()) {
728	it.next();
729	cmbCorr* corr = it.value();
730
731	t_orbCorr orbCorr(corr->_orbCorr);
732	orbCorr._staID = "INTERNAL";
733	orbCorrections.push_back(orbCorr);
734
735	t_clkCorr clkCorr(corr->_clkCorr);
736	clkCorr._staID = "INTERNAL";
737	clkCorr._dClk = corr->_dClkResult;
738	clkCorr._dotDClk = 0.0;
739	clkCorr._dotDotDClk = 0.0;
740	clkCorrections.push_back(clkCorr);
741
742	ColumnVector xc(6);
743	ColumnVector vv(3);
744	corr->_eph->setClkCorr(dynamic_cast<const t_clkCorr*>(&clkCorr));
745	corr->_eph->setOrbCorr(dynamic_cast<const t_orbCorr*>(&orbCorr));
746	corr->_eph->getCrd(_resTime, xc, vv, true);
747
748	// Correction Phase Center --> CoM
749	// -------------------------------
750	ColumnVector dx(3); dx = 0.0;
751	if (_antex) {
752	double Mjd = _resTime.mjd() + _resTime.daysec()/86400.0;
753	if (_antex->satCoMcorrection(corr->_prn, Mjd, xc.Rows(1,3), dx) != success) {
754	dx = 0;
755	_log += "antenna not found " + corr->_prn.mid(0,3).toLatin1() + '\n';
756	}
757	}
758
759	outLines += corr->_prn.mid(0,3);
760	QString hlp;
761	hlp.sprintf(" APC 3 %15.4f %15.4f %15.4f"
762	" Clk 1 %15.4f"
763	" Vel 3 %15.4f %15.4f %15.4f"
764	" CoM 3 %15.4f %15.4f %15.4f\n",
765	xc(1), xc(2), xc(3),
766	xc(4) * t_CST::c,
767	vv(1), vv(2), vv(3),
768	xc(1)-dx(1), xc(2)-dx(2), xc(3)-dx(3));
769	outLines += hlp;
770
771	QString line;
772	int messageType = COTYPE_GPSCOMBINED;
773	int updateInt = 0;
774	line.sprintf("%d %d %d %.1f %s"
775	" %lu"
776	" %8.3f %8.3f %8.3f %8.3f"
777	" %10.5f %10.5f %10.5f %10.5f"
778	" %10.5f INTERNAL",
779	messageType, updateInt, _resTime.gpsw(), _resTime.gpssec(),
780	corr->_prn.mid(0,3).toLatin1().data(),
781	corr->_iod,
782	corr->_dClkResult * t_CST::c,
783	corr->_orbCorr._xr[0],
784	corr->_orbCorr._xr[1],
785	corr->_orbCorr._xr[2],
786	0.0,
787	corr->_orbCorr._dotXr[0],
788	corr->_orbCorr._dotXr[1],
789	corr->_orbCorr._dotXr[2],
790	0.0);
791	corrLines << line;
792
793	delete corr;
794	}
795
796	outLines += "EOE\n"; // End Of Epoch flag
797
798	if (!_rtnetDecoder) {
799	_rtnetDecoder = new bncRtnetDecoder();
800	}
801
802	vector<string> errmsg;
803	_rtnetDecoder->Decode(outLines.toLatin1().data(), outLines.length(), errmsg);
804
805	// Send new Corrections to PPP etc.
806	// --------------------------------
807	if (orbCorrections.size() > 0 && clkCorrections.size() > 0) {
808	emit newOrbCorrections(orbCorrections);
809	emit newClkCorrections(clkCorrections);
810	}
811	}
812
813	// Create First Design Matrix and Vector of Measurements
814	////////////////////////////////////////////////////////////////////////////
815	t_irc bncComb::createAmat(Matrix& AA, ColumnVector& ll, DiagonalMatrix& PP,
816	const ColumnVector& x0,
817	QMap<QString, cmbCorr*>& resCorr) {
818
819	unsigned nPar = _params.size();
820	unsigned nObs = corrs().size();
821
822	if (nObs == 0) {
823	return failure;
824	}
825
826	int maxSat = t_prn::MAXPRN_GPS;
827	// if (_useGlonass) {
828	// maxSat = t_prn::MAXPRN_GPS + t_prn::MAXPRN_GLONASS;
829	// }
830
831	const int nCon = (_method == filter) ? 1 + maxSat : 0;
832
833	AA.ReSize(nObs+nCon, nPar); AA = 0.0;
834	ll.ReSize(nObs+nCon); ll = 0.0;
835	PP.ReSize(nObs+nCon); PP = 1.0 / (sigObs * sigObs);
836
837	int iObs = 0;
838	QVectorIterator<cmbCorr*> itCorr(corrs());
839	while (itCorr.hasNext()) {
840	cmbCorr* corr = itCorr.next();
841	QString prn = corr->_prn;
842
843	++iObs;
844
845	if (corr->_acName == _masterOrbitAC && resCorr.find(prn) == resCorr.end()) {
846	resCorr[prn] = new cmbCorr(*corr);
847	}
848
849	for (int iPar = 1; iPar <= _params.size(); iPar++) {
850	cmbParam* pp = _params[iPar-1];
851	AA(iObs, iPar) = pp->partial(corr->_acName, prn);
852	}
853
854	ll(iObs) = corr->_clkCorr._dClk * t_CST::c - DotProduct(AA.Row(iObs), x0);
855	}
856
857	// Regularization
858	// --------------
859	if (_method == filter) {
860	const double Ph = 1.e6;
861	PP(nObs+1) = Ph;
862	for (int iPar = 1; iPar <= _params.size(); iPar++) {
863	cmbParam* pp = _params[iPar-1];
864	if ( AA.Column(iPar).maximum_absolute_value() > 0.0 &&
865	pp->type == cmbParam::clkSat ) {
866	AA(nObs+1, iPar) = 1.0;
867	}
868	}
869	int iCond = 1;
870	for (unsigned iGps = 1; iGps <= t_prn::MAXPRN_GPS; iGps++) {
871	QString prn = QString("G%1_0").arg(iGps, 2, 10, QChar('0'));
872	++iCond;
873	PP(nObs+iCond) = Ph;
874	for (int iPar = 1; iPar <= _params.size(); iPar++) {
875	cmbParam* pp = _params[iPar-1];
876	if ( pp &&
877	AA.Column(iPar).maximum_absolute_value() > 0.0 &&
878	pp->type == cmbParam::offACSat &&
879	pp->prn == prn) {
880	AA(nObs+iCond, iPar) = 1.0;
881	}
882	}
883	}
884	// if (_useGlonass) {// liefert schlechtere Höhenkomponente im PPP
885	// for (unsigned iGlo = 1; iGlo <= t_prn::MAXPRN_GLONASS; iGlo++) {
886	// QString prn = QString("R%1_0").arg(iGlo, 2, 10, QChar('0'));
887	// ++iCond;
888	// PP(nObs+iCond) = Ph;
889	// for (int iPar = 1; iPar <= _params.size(); iPar++) {
890	// cmbParam* pp = _params[iPar-1];
891	// if ( pp &&
892	// AA.Column(iPar).maximum_absolute_value() > 0.0 &&
893	// pp->type == cmbParam::offACSat &&
894	// pp->prn == prn) {
895	// AA(nObs+iCond, iPar) = 1.0;
896	// }
897	// }
898	// }
899	// }
900	}
901
902	return success;
903	}
904
905	// Process Epoch - Single-Epoch Method
906	////////////////////////////////////////////////////////////////////////////
907	t_irc bncComb::processEpoch_singleEpoch(QTextStream& out,
908	QMap<QString, cmbCorr*>& resCorr,
909	ColumnVector& dx) {
910
911	// Check Satellite Positions for Outliers
912	// --------------------------------------
913	if (checkOrbits(out) != success) {
914	return failure;
915	}
916
917	// Outlier Detection Loop
918	// ----------------------
919	while (true) {
920
921	// Remove Satellites that are not in Master
922	// ----------------------------------------
923	QMutableVectorIterator<cmbCorr*> it(corrs());
924	while (it.hasNext()) {
925	cmbCorr* corr = it.next();
926	QString prn = corr->_prn;
927	bool foundMaster = false;
928	QVectorIterator<cmbCorr*> itHlp(corrs());
929	while (itHlp.hasNext()) {
930	cmbCorr* corrHlp = itHlp.next();
931	QString prnHlp = corrHlp->_prn;
932	QString ACHlp = corrHlp->_acName;
933	if (ACHlp == _masterOrbitAC && prn == prnHlp) {
934	foundMaster = true;
935	break;
936	}
937	}
938	if (!foundMaster) {
939	delete corr;
940	it.remove();
941	}
942	}
943
944	// Count Number of Observations per Satellite and per AC
945	// -----------------------------------------------------
946	QMap<QString, int> numObsPrn;
947	QMap<QString, int> numObsAC;
948	QVectorIterator<cmbCorr*> itCorr(corrs());
949	while (itCorr.hasNext()) {
950	cmbCorr* corr = itCorr.next();
951	QString prn = corr->_prn;
952	QString AC = corr->_acName;
953	if (numObsPrn.find(prn) == numObsPrn.end()) {
954	numObsPrn[prn] = 1;
955	}
956	else {
957	numObsPrn[prn] += 1;
958	}
959	if (numObsAC.find(AC) == numObsAC.end()) {
960	numObsAC[AC] = 1;
961	}
962	else {
963	numObsAC[AC] += 1;
964	}
965	}
966
967	// Clean-Up the Paramters
968	// ----------------------
969	for (int iPar = 1; iPar <= _params.size(); iPar++) {
970	delete _params[iPar-1];
971	}
972	_params.clear();
973
974	// Set new Parameters
975	// ------------------
976	int nextPar = 0;
977
978	QMapIterator<QString, int> itAC(numObsAC);
979	while (itAC.hasNext()) {
980	itAC.next();
981	const QString& AC = itAC.key();
982	int numObs = itAC.value();
983	if (AC != _masterOrbitAC && numObs > 0) {
984	_params.push_back(new cmbParam(cmbParam::offACgps, ++nextPar, AC, ""));
985	if (_useGlonass) {
986	_params.push_back(new cmbParam(cmbParam::offACglo, ++nextPar, AC, ""));
987	}
988	}
989	}
990
991	QMapIterator<QString, int> itPrn(numObsPrn);
992	while (itPrn.hasNext()) {
993	itPrn.next();
994	const QString& prn = itPrn.key();
995	int numObs = itPrn.value();
996	if (numObs > 0) {
997	_params.push_back(new cmbParam(cmbParam::clkSat, ++nextPar, "", prn));
998	}
999	}
1000
1001	int nPar = _params.size();
1002	ColumnVector x0(nPar);
1003	x0 = 0.0;
1004
1005	// Create First-Design Matrix
1006	// --------------------------
1007	Matrix AA;
1008	ColumnVector ll;
1009	DiagonalMatrix PP;
1010	if (createAmat(AA, ll, PP, x0, resCorr) != success) {
1011	return failure;
1012	}
1013
1014	ColumnVector vv;
1015	try {
1016	Matrix ATP = AA.t() * PP;
1017	SymmetricMatrix NN; NN << ATP * AA;
1018	ColumnVector bb = ATP * ll;
1019	_QQ = NN.i();
1020	dx = _QQ * bb;
1021	vv = ll - AA * dx;
1022	}
1023	catch (Exception& exc) {
1024	out << exc.what() << endl;
1025	return failure;
1026	}
1027
1028	int maxResIndex;
1029	double maxRes = vv.maximum_absolute_value1(maxResIndex);
1030	out.setRealNumberNotation(QTextStream::FixedNotation);
1031	out.setRealNumberPrecision(3);
1032	out << _resTime.datestr().c_str() << " " << _resTime.timestr().c_str()
1033	<< " Maximum Residuum " << maxRes << ' '
1034	<< corrs()[maxResIndex-1]->_acName << ' ' << corrs()[maxResIndex-1]->_prn.mid(0,3);
1035
1036	if (maxRes > _MAXRES) {
1037	out << " Outlier" << endl;
1038	delete corrs()[maxResIndex-1];
1039	corrs().remove(maxResIndex-1);
1040	}
1041	else {
1042	out << " OK" << endl;
1043	out.setRealNumberNotation(QTextStream::FixedNotation);
1044	out.setRealNumberPrecision(3);
1045	for (int ii = 0; ii < vv.Nrows(); ii++) {
1046	const cmbCorr* corr = corrs()[ii];
1047	out << _resTime.datestr().c_str() << ' '
1048	<< _resTime.timestr().c_str() << " "
1049	<< corr->_acName << ' ' << corr->_prn.mid(0,3);
1050	out.setFieldWidth(6);
1051	out << " res = " << vv[ii] << endl;
1052	out.setFieldWidth(0);
1053	}
1054	return success;
1055	}
1056
1057	}
1058
1059	return failure;
1060	}
1061
1062	// Check Satellite Positions for Outliers
1063	////////////////////////////////////////////////////////////////////////////
1064	t_irc bncComb::checkOrbits(QTextStream& out) {
1065
1066	const double MAX_DISPLACEMENT = 0.20;
1067
1068	// Switch to last ephemeris (if possible)
1069	// --------------------------------------
1070	QMutableVectorIterator<cmbCorr*> im(corrs());
1071	while (im.hasNext()) {
1072	cmbCorr* corr = im.next();
1073	QString prn = corr->_prn;
1074
1075	t_eph* ephLast = _ephUser.ephLast(prn);
1076	t_eph* ephPrev = _ephUser.ephPrev(prn);
1077
1078	if (ephLast == 0) {
1079	out << "checkOrbit: missing eph (not found) " << corr->_prn.mid(0,3) << endl;
1080	delete corr;
1081	im.remove();
1082	}
1083	else if (corr->_eph == 0) {
1084	out << "checkOrbit: missing eph (zero) " << corr->_prn.mid(0,3) << endl;
1085	delete corr;
1086	im.remove();
1087	}
1088	else {
1089	if ( corr->_eph == ephLast \|\| corr->_eph == ephPrev ) {
1090	switchToLastEph(ephLast, corr);
1091	}
1092	else {
1093	out << "checkOrbit: missing eph (deleted) " << corr->_prn.mid(0,3) << endl;
1094	delete corr;
1095	im.remove();
1096	}
1097	}
1098	}
1099
1100	while (true) {
1101
1102	// Compute Mean Corrections for all Satellites
1103	// -------------------------------------------
1104	QMap<QString, int> numCorr;
1105	QMap<QString, ColumnVector> meanRao;
1106	QVectorIterator<cmbCorr*> it(corrs());
1107	while (it.hasNext()) {
1108	cmbCorr* corr = it.next();
1109	QString prn = corr->_prn;
1110	if (meanRao.find(prn) == meanRao.end()) {
1111	meanRao[prn].ReSize(4);
1112	meanRao[prn].Rows(1,3) = corr->_orbCorr._xr;
1113	meanRao[prn](4) = 1;
1114	}
1115	else {
1116	meanRao[prn].Rows(1,3) += corr->_orbCorr._xr;
1117	meanRao[prn](4) += 1;
1118	}
1119	if (numCorr.find(prn) == numCorr.end()) {
1120	numCorr[prn] = 1;
1121	}
1122	else {
1123	numCorr[prn] += 1;
1124	}
1125	}
1126
1127	// Compute Differences wrt Mean, find Maximum
1128	// ------------------------------------------
1129	QMap<QString, cmbCorr*> maxDiff;
1130	it.toFront();
1131	while (it.hasNext()) {
1132	cmbCorr* corr = it.next();
1133	QString prn = corr->_prn;
1134	if (meanRao[prn](4) != 0) {
1135	meanRao[prn] /= meanRao[prn](4);
1136	meanRao[prn](4) = 0;
1137	}
1138	corr->_diffRao = corr->_orbCorr._xr - meanRao[prn].Rows(1,3);
1139	if (maxDiff.find(prn) == maxDiff.end()) {
1140	maxDiff[prn] = corr;
1141	}
1142	else {
1143	double normMax = maxDiff[prn]->_diffRao.NormFrobenius();
1144	double norm = corr->_diffRao.NormFrobenius();
1145	if (norm > normMax) {
1146	maxDiff[prn] = corr;
1147	}
1148	}
1149	}
1150
1151	if (_ACs.size() == 1) {
1152	break;
1153	}
1154
1155	// Remove Outliers
1156	// ---------------
1157	bool removed = false;
1158	QMutableVectorIterator<cmbCorr*> im(corrs());
1159	while (im.hasNext()) {
1160	cmbCorr* corr = im.next();
1161	QString prn = corr->_prn;
1162	if (numCorr[prn] < 2) {
1163	delete corr;
1164	im.remove();
1165	}
1166	else if (corr == maxDiff[prn]) {
1167	double norm = corr->_diffRao.NormFrobenius();
1168	if (norm > MAX_DISPLACEMENT) {
1169	out << _resTime.datestr().c_str() << " "
1170	<< _resTime.timestr().c_str() << " "
1171	<< "Orbit Outlier: "
1172	<< corr->_acName.toLatin1().data() << " "
1173	<< prn.mid(0,3).toLatin1().data() << " "
1174	<< corr->_iod << " "
1175	<< norm << endl;
1176	delete corr;
1177	im.remove();
1178	removed = true;
1179	}
1180	}
1181	}
1182
1183	if (!removed) {
1184	break;
1185	}
1186	}
1187
1188	return success;
1189	}
1190
1191	//
1192	////////////////////////////////////////////////////////////////////////////
1193	void bncComb::slotProviderIDChanged(QString mountPoint) {
1194	QMutexLocker locker(&_mutex);
1195
1196	QTextStream out(&_log, QIODevice::WriteOnly);
1197
1198	// Find the AC Name
1199	// ----------------
1200	QString acName;
1201	QListIterator<cmbAC*> icAC(_ACs);
1202	while (icAC.hasNext()) {
1203	cmbAC* AC = icAC.next();
1204	if (AC->mountPoint == mountPoint) {
1205	acName = AC->name;
1206	out << "Provider ID changed: AC " << AC->name.toLatin1().data() << " "
1207	<< _resTime.datestr().c_str() << " "
1208	<< _resTime.timestr().c_str() << endl;
1209	break;
1210	}
1211	}
1212	if (acName.isEmpty()) {
1213	return;
1214	}
1215
1216	// Remove all corrections of the corresponding AC
1217	// ----------------------------------------------
1218	QListIterator<bncTime> itTime(_buffer.keys());
1219	while (itTime.hasNext()) {
1220	bncTime epoTime = itTime.next();
1221	QVector<cmbCorr*>& corrVec = _buffer[epoTime].corrs;
1222	QMutableVectorIterator<cmbCorr*> it(corrVec);
1223	while (it.hasNext()) {
1224	cmbCorr* corr = it.next();
1225	if (acName == corr->_acName) {
1226	delete corr;
1227	it.remove();
1228	}
1229	}
1230	}
1231
1232	// Reset Satellite Offsets
1233	// -----------------------
1234	if (_method == filter) {
1235	for (int iPar = 1; iPar <= _params.size(); iPar++) {
1236	cmbParam* pp = _params[iPar-1];
1237	if (pp->AC == acName && pp->type == cmbParam::offACSat) {
1238	pp->xx = 0.0;
1239	_QQ.Row(iPar) = 0.0;
1240	_QQ.Column(iPar) = 0.0;
1241	_QQ(iPar,iPar) = pp->sig0 * pp->sig0;
1242	}
1243	}
1244	}
1245	}

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

Download in other formats: