Context Navigation

source: ntrip/branches/BNC_2.12/src/combination/bnccomb.cpp@ 7933

Last change on this file since 7933 was 7715, checked in by stuerze, 9 years ago
minor changes
File size: 34.4 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	// Check Modulo Time
351	// -----------------
352	if (int(clkCorr._time.gpssec()) % _cmbSampl != 0.0) {
353	continue;
354	}
355
356	// Check Correction Age
357	// --------------------
358	if (_resTime.valid() && clkCorr._time <= _resTime) {
359	emit newMessage("bncComb: old correction: " + acName.toAscii() + " " + prn.mid(0,3).toAscii(), true);
360	continue;
361	}
362
363	// Create new correction
364	// ---------------------
365	cmbCorr* newCorr = new cmbCorr();
366	newCorr->_prn = prn;
367	newCorr->_time = clkCorr._time;
368	newCorr->_iod = clkCorr._iod;
369	newCorr->_acName = acName;
370	newCorr->_clkCorr = clkCorr;
371
372	// Check orbit correction
373	// ----------------------
374	if (!_orbCorrections.contains(acName)) {
375	delete newCorr;
376	continue;
377	}
378	else {
379	QMap<t_prn, t_orbCorr>& storage = _orbCorrections[acName];
380	if (!storage.contains(clkCorr._prn) \|\| storage[clkCorr._prn]._iod != newCorr->_iod) {
381	delete newCorr;
382	continue;
383	}
384	else {
385	newCorr->_orbCorr = storage[clkCorr._prn];
386	}
387	}
388
389	// Check the Ephemeris
390	//--------------------
391	t_eph* ephLast = _ephUser.ephLast(prn);
392	t_eph* ephPrev = _ephUser.ephPrev(prn);
393	if (ephLast == 0) {
394	emit newMessage("bncComb: eph not found " + prn.mid(0,3).toAscii(), true);
395	delete newCorr;
396	continue;
397	}
398	else {
399	if (ephLast->IOD() == newCorr->_iod) {
400	newCorr->_eph = ephLast;
401	}
402	else if (ephPrev && ephPrev->IOD() == newCorr->_iod) {
403	newCorr->_eph = ephPrev;
404	switchToLastEph(ephLast, newCorr);
405	}
406	else {
407	emit newMessage("bncComb: eph not found " + prn.mid(0,3).toAscii() +
408	QString(" %1").arg(newCorr->_iod).toAscii(), true);
409	delete newCorr;
410	continue;
411	}
412	}
413
414	// Store correction into the buffer
415	// --------------------------------
416	QVector<cmbCorr*>& corrs = _buffer[newCorr->_time].corrs;
417	corrs.push_back(newCorr);
418	}
419
420	// Process previous Epoch(s)
421	// -------------------------
422	const double outWait = 1.0 * _cmbSampl;
423	QListIterator<bncTime> itTime(_buffer.keys());
424	while (itTime.hasNext()) {
425	bncTime epoTime = itTime.next();
426	if (epoTime < lastTime - outWait) {
427	_resTime = epoTime;
428	processEpoch();
429	}
430	}
431	}
432
433	// Change the correction so that it refers to last received ephemeris
434	////////////////////////////////////////////////////////////////////////////
435	void bncComb::switchToLastEph(t_eph* lastEph, cmbCorr* corr) {
436
437	if (corr->_eph == lastEph) {
438	return;
439	}
440
441	ColumnVector oldXC(4);
442	ColumnVector oldVV(3);
443	corr->_eph->getCrd(corr->_time, oldXC, oldVV, false);
444
445	ColumnVector newXC(4);
446	ColumnVector newVV(3);
447	lastEph->getCrd(corr->_time, newXC, newVV, false);
448
449	ColumnVector dX = newXC.Rows(1,3) - oldXC.Rows(1,3);
450	ColumnVector dV = newVV - oldVV;
451	double dC = newXC(4) - oldXC(4);
452
453	ColumnVector dRAO(3);
454	XYZ_to_RSW(newXC.Rows(1,3), newVV, dX, dRAO);
455
456	ColumnVector dDotRAO(3);
457	XYZ_to_RSW(newXC.Rows(1,3), newVV, dV, dDotRAO);
458
459	QString msg = "switch corr " + corr->_prn.mid(0,3)
460	+ QString(" %1 -> %2 %3").arg(corr->_iod,3).arg(lastEph->IOD(),3).arg(dC*t_CST::c, 8, 'f', 4);
461
462	emit newMessage(msg.toAscii(), false);
463
464	corr->_iod = lastEph->IOD();
465	corr->_eph = lastEph;
466
467	corr->_orbCorr._xr += dRAO;
468	corr->_orbCorr._dotXr += dDotRAO;
469	corr->_clkCorr._dClk -= dC;
470	}
471
472	// Process Epoch
473	////////////////////////////////////////////////////////////////////////////
474	void bncComb::processEpoch() {
475
476	_log.clear();
477
478	QTextStream out(&_log, QIODevice::WriteOnly);
479
480	out << endl << "Combination:" << endl
481	<< "------------------------------" << endl;
482
483	// Observation Statistics
484	// ----------------------
485	bool masterPresent = false;
486	QListIterator<cmbAC*> icAC(_ACs);
487	while (icAC.hasNext()) {
488	cmbAC* AC = icAC.next();
489	AC->numObs = 0;
490	QVectorIterator<cmbCorr*> itCorr(corrs());
491	while (itCorr.hasNext()) {
492	cmbCorr* corr = itCorr.next();
493	if (corr->_acName == AC->name) {
494	AC->numObs += 1;
495	if (AC->name == _masterOrbitAC) {
496	masterPresent = true;
497	}
498	}
499	}
500	out << AC->name.toAscii().data() << ": " << AC->numObs << endl;
501	}
502
503	// If Master not present, switch to another one
504	// --------------------------------------------
505	const unsigned switchMasterAfterGap = 1;
506	if (masterPresent) {
507	_masterMissingEpochs = 0;
508	}
509	else {
510	++_masterMissingEpochs;
511	if (_masterMissingEpochs < switchMasterAfterGap) {
512	out << "Missing Master, Epoch skipped" << endl;
513	_buffer.remove(_resTime);
514	emit newMessage(_log, false);
515	return;
516	}
517	else {
518	_masterMissingEpochs = 0;
519	QListIterator<cmbAC*> icAC(_ACs);
520	while (icAC.hasNext()) {
521	cmbAC* AC = icAC.next();
522	if (AC->numObs > 0) {
523	out << "Switching Master AC "
524	<< _masterOrbitAC.toAscii().data() << " --> "
525	<< AC->name.toAscii().data() << " "
526	<< _resTime.datestr().c_str() << " "
527	<< _resTime.timestr().c_str() << endl;
528	_masterOrbitAC = AC->name;
529	break;
530	}
531	}
532	}
533	}
534
535	QMap<QString, cmbCorr*> resCorr;
536
537	// Perform the actual Combination using selected Method
538	// ----------------------------------------------------
539	t_irc irc;
540	ColumnVector dx;
541	if (_method == filter) {
542	irc = processEpoch_filter(out, resCorr, dx);
543	}
544	else {
545	irc = processEpoch_singleEpoch(out, resCorr, dx);
546	}
547
548	// Update Parameter Values, Print Results
549	// --------------------------------------
550	if (irc == success) {
551	for (int iPar = 1; iPar <= _params.size(); iPar++) {
552	cmbParam* pp = _params[iPar-1];
553	pp->xx += dx(iPar);
554	if (pp->type == cmbParam::clkSat) {
555	if (resCorr.find(pp->prn) != resCorr.end()) {
556	resCorr[pp->prn]->_dClkResult = pp->xx / t_CST::c;
557	}
558	}
559	out << _resTime.datestr().c_str() << " "
560	<< _resTime.timestr().c_str() << " ";
561	out.setRealNumberNotation(QTextStream::FixedNotation);
562	out.setFieldWidth(8);
563	out.setRealNumberPrecision(4);
564	out << pp->toString() << " "
565	<< pp->xx << " +- " << sqrt(_QQ(pp->index,pp->index)) << endl;
566	out.setFieldWidth(0);
567	}
568	printResults(out, resCorr);
569	dumpResults(resCorr);
570	}
571
572	// Delete Data, emit Message
573	// -------------------------
574	_buffer.remove(_resTime);
575	emit newMessage(_log, false);
576	}
577
578	// Process Epoch - Filter Method
579	////////////////////////////////////////////////////////////////////////////
580	t_irc bncComb::processEpoch_filter(QTextStream& out,
581	QMap<QString, cmbCorr*>& resCorr,
582	ColumnVector& dx) {
583
584	// Prediction Step
585	// ---------------
586	int nPar = _params.size();
587	ColumnVector x0(nPar);
588	for (int iPar = 1; iPar <= _params.size(); iPar++) {
589	cmbParam* pp = _params[iPar-1];
590	if (pp->epoSpec) {
591	pp->xx = 0.0;
592	_QQ.Row(iPar) = 0.0;
593	_QQ.Column(iPar) = 0.0;
594	_QQ(iPar,iPar) = pp->sig0 * pp->sig0;
595	}
596	else {
597	_QQ(iPar,iPar) += pp->sigP * pp->sigP;
598	}
599	x0(iPar) = pp->xx;
600	}
601
602	// Check Satellite Positions for Outliers
603	// --------------------------------------
604	if (checkOrbits(out) != success) {
605	return failure;
606	}
607
608	// Update and outlier detection loop
609	// ---------------------------------
610	SymmetricMatrix QQ_sav = _QQ;
611	while (true) {
612
613	Matrix AA;
614	ColumnVector ll;
615	DiagonalMatrix PP;
616
617	if (createAmat(AA, ll, PP, x0, resCorr) != success) {
618	return failure;
619	}
620
621	dx.ReSize(nPar); dx = 0.0;
622	kalman(AA, ll, PP, _QQ, dx);
623
624	ColumnVector vv = ll - AA * dx;
625
626	int maxResIndex;
627	double maxRes = vv.maximum_absolute_value1(maxResIndex);
628	out.setRealNumberNotation(QTextStream::FixedNotation);
629	out.setRealNumberPrecision(3);
630	out << _resTime.datestr().c_str() << " " << _resTime.timestr().c_str()
631	<< " Maximum Residuum " << maxRes << ' '
632	<< corrs()[maxResIndex-1]->_acName << ' ' << corrs()[maxResIndex-1]->_prn.mid(0,3);
633	if (maxRes > _MAXRES) {
634	for (int iPar = 1; iPar <= _params.size(); iPar++) {
635	cmbParam* pp = _params[iPar-1];
636	if (pp->type == cmbParam::offACSat &&
637	pp->AC == corrs()[maxResIndex-1]->_acName &&
638	pp->prn == corrs()[maxResIndex-1]->_prn.mid(0,3)) {
639	QQ_sav.Row(iPar) = 0.0;
640	QQ_sav.Column(iPar) = 0.0;
641	QQ_sav(iPar,iPar) = pp->sig0 * pp->sig0;
642	}
643	}
644
645	out << " Outlier" << endl;
646	_QQ = QQ_sav;
647	corrs().remove(maxResIndex-1);
648	}
649	else {
650	out << " OK" << endl;
651	out.setRealNumberNotation(QTextStream::FixedNotation);
652	out.setRealNumberPrecision(4);
653	for (int ii = 0; ii < corrs().size(); ii++) {
654	const cmbCorr* corr = corrs()[ii];
655	out << _resTime.datestr().c_str() << ' '
656	<< _resTime.timestr().c_str() << " "
657	<< corr->_acName << ' ' << corr->_prn.mid(0,3);
658	out.setFieldWidth(10);
659	out << " res = " << vv[ii] << endl;
660	out.setFieldWidth(0);
661	}
662	break;
663	}
664	}
665
666	return success;
667	}
668
669	// Print results
670	////////////////////////////////////////////////////////////////////////////
671	void bncComb::printResults(QTextStream& out,
672	const QMap<QString, cmbCorr*>& resCorr) {
673
674	QMapIterator<QString, cmbCorr*> it(resCorr);
675	while (it.hasNext()) {
676	it.next();
677	cmbCorr* corr = it.value();
678	const t_eph* eph = corr->_eph;
679	if (eph) {
680	ColumnVector xc(4);
681	ColumnVector vv(3);
682	eph->getCrd(_resTime, xc, vv, false);
683
684	out << _resTime.datestr().c_str() << " "
685	<< _resTime.timestr().c_str() << " ";
686	out.setFieldWidth(3);
687	out << "Full Clock " << corr->_prn.mid(0,3) << " " << corr->_iod << " ";
688	out.setFieldWidth(14);
689	out << (xc(4) + corr->_dClkResult) * t_CST::c << endl;
690	out.setFieldWidth(0);
691	}
692	else {
693	out << "bncComb::printResuls bug" << endl;
694	}
695	}
696	}
697
698	// Send results to RTNet Decoder and directly to PPP Client
699	////////////////////////////////////////////////////////////////////////////
700	void bncComb::dumpResults(const QMap<QString, cmbCorr*>& resCorr) {
701
702	QList<t_orbCorr> orbCorrections;
703	QList<t_clkCorr> clkCorrections;
704
705	QString outLines;
706	QStringList corrLines;
707
708	unsigned year, month, day, hour, minute;
709	double sec;
710	_resTime.civil_date(year, month, day);
711	_resTime.civil_time(hour, minute, sec);
712
713	outLines.sprintf("* %4d %2d %2d %d %d %12.8f\n",
714	year, month, day, hour, minute, sec);
715
716	QMapIterator<QString, cmbCorr*> it(resCorr);
717	while (it.hasNext()) {
718	it.next();
719	cmbCorr* corr = it.value();
720
721	t_orbCorr orbCorr(corr->_orbCorr);
722	orbCorr._staID = "INTERNAL";
723	orbCorrections.push_back(orbCorr);
724
725	t_clkCorr clkCorr(corr->_clkCorr);
726	clkCorr._staID = "INTERNAL";
727	clkCorr._dClk = corr->_dClkResult;
728	clkCorr._dotDClk = 0.0;
729	clkCorr._dotDotDClk = 0.0;
730	clkCorrections.push_back(clkCorr);
731
732	ColumnVector xc(4);
733	ColumnVector vv(3);
734	corr->_eph->setClkCorr(dynamic_cast<const t_clkCorr*>(&clkCorr));
735	corr->_eph->setOrbCorr(dynamic_cast<const t_orbCorr*>(&orbCorr));
736	corr->_eph->getCrd(_resTime, xc, vv, true);
737
738	// Correction Phase Center --> CoM
739	// -------------------------------
740	ColumnVector dx(3); dx = 0.0;
741	if (_antex) {
742	double Mjd = _resTime.mjd() + _resTime.daysec()/86400.0;
743	if (_antex->satCoMcorrection(corr->_prn, Mjd, xc.Rows(1,3), dx) != success) {
744	dx = 0;
745	_log += "antenna not found " + corr->_prn.mid(0,3).toAscii() + '\n';
746	}
747	}
748
749	outLines += corr->_prn.mid(0,3);
750	QString hlp;
751	hlp.sprintf(" APC 3 %15.4f %15.4f %15.4f"
752	" Clk 1 %15.4f"
753	" Vel 3 %15.4f %15.4f %15.4f"
754	" CoM 3 %15.4f %15.4f %15.4f\n",
755	xc(1), xc(2), xc(3),
756	xc(4) * t_CST::c,
757	vv(1), vv(2), vv(3),
758	xc(1)-dx(1), xc(2)-dx(2), xc(3)-dx(3));
759	outLines += hlp;
760
761	QString line;
762	int messageType = COTYPE_GPSCOMBINED;
763	int updateInt = 0;
764	line.sprintf("%d %d %d %.1f %s"
765	" %lu"
766	" %8.3f %8.3f %8.3f %8.3f"
767	" %10.5f %10.5f %10.5f %10.5f"
768	" %10.5f INTERNAL",
769	messageType, updateInt, _resTime.gpsw(), _resTime.gpssec(),
770	corr->_prn.mid(0,3).toAscii().data(),
771	corr->_iod,
772	corr->_dClkResult * t_CST::c,
773	corr->_orbCorr._xr[0],
774	corr->_orbCorr._xr[1],
775	corr->_orbCorr._xr[2],
776	0.0,
777	corr->_orbCorr._dotXr[0],
778	corr->_orbCorr._dotXr[1],
779	corr->_orbCorr._dotXr[2],
780	0.0);
781	corrLines << line;
782
783	delete corr;
784	}
785
786	outLines += "EOE\n"; // End Of Epoch flag
787
788	if (!_rtnetDecoder) {
789	_rtnetDecoder = new bncRtnetDecoder();
790	}
791
792	vector<string> errmsg;
793	_rtnetDecoder->Decode(outLines.toAscii().data(), outLines.length(), errmsg);
794
795	// Send new Corrections to PPP etc.
796	// --------------------------------
797	if (orbCorrections.size() > 0 && clkCorrections.size() > 0) {
798	emit newOrbCorrections(orbCorrections);
799	emit newClkCorrections(clkCorrections);
800	}
801	}
802
803	// Create First Design Matrix and Vector of Measurements
804	////////////////////////////////////////////////////////////////////////////
805	t_irc bncComb::createAmat(Matrix& AA, ColumnVector& ll, DiagonalMatrix& PP,
806	const ColumnVector& x0,
807	QMap<QString, cmbCorr*>& resCorr) {
808
809	unsigned nPar = _params.size();
810	unsigned nObs = corrs().size();
811
812	if (nObs == 0) {
813	return failure;
814	}
815
816	int maxSat = t_prn::MAXPRN_GPS;
817	// if (_useGlonass) {
818	// maxSat = t_prn::MAXPRN_GPS + t_prn::MAXPRN_GLONASS;
819	// }
820
821	const int nCon = (_method == filter) ? 1 + maxSat : 0;
822
823	AA.ReSize(nObs+nCon, nPar); AA = 0.0;
824	ll.ReSize(nObs+nCon); ll = 0.0;
825	PP.ReSize(nObs+nCon); PP = 1.0 / (sigObs * sigObs);
826
827	int iObs = 0;
828	QVectorIterator<cmbCorr*> itCorr(corrs());
829	while (itCorr.hasNext()) {
830	cmbCorr* corr = itCorr.next();
831	QString prn = corr->_prn;
832
833	++iObs;
834
835	if (corr->_acName == _masterOrbitAC && resCorr.find(prn) == resCorr.end()) {
836	resCorr[prn] = new cmbCorr(*corr);
837	}
838
839	for (int iPar = 1; iPar <= _params.size(); iPar++) {
840	cmbParam* pp = _params[iPar-1];
841	AA(iObs, iPar) = pp->partial(corr->_acName, prn);
842	}
843
844	ll(iObs) = corr->_clkCorr._dClk * t_CST::c - DotProduct(AA.Row(iObs), x0);
845	}
846
847	// Regularization
848	// --------------
849	if (_method == filter) {
850	const double Ph = 1.e6;
851	PP(nObs+1) = Ph;
852	for (int iPar = 1; iPar <= _params.size(); iPar++) {
853	cmbParam* pp = _params[iPar-1];
854	if ( AA.Column(iPar).maximum_absolute_value() > 0.0 &&
855	pp->type == cmbParam::clkSat ) {
856	AA(nObs+1, iPar) = 1.0;
857	}
858	}
859	int iCond = 1;
860	for (unsigned iGps = 1; iGps <= t_prn::MAXPRN_GPS; iGps++) {
861	QString prn = QString("G%1_0").arg(iGps, 2, 10, QChar('0'));
862	++iCond;
863	PP(nObs+iCond) = Ph;
864	for (int iPar = 1; iPar <= _params.size(); iPar++) {
865	cmbParam* pp = _params[iPar-1];
866	if ( pp &&
867	AA.Column(iPar).maximum_absolute_value() > 0.0 &&
868	pp->type == cmbParam::offACSat &&
869	pp->prn == prn) {
870	AA(nObs+iCond, iPar) = 1.0;
871	}
872	}
873	}
874	// if (_useGlonass) {
875	// for (int iGlo = 1; iGlo <= t_prn::MAXPRN_GLONASS; iGlo++) {
876	// QString prn = QString("R%1_0").arg(iGlo, 2, 10, QChar('0'));
877	// ++iCond;
878	// PP(nObs+iCond) = Ph;
879	// for (int iPar = 1; iPar <= _params.size(); iPar++) {
880	// cmbParam* pp = _params[iPar-1];
881	// if ( pp &&
882	// AA.Column(iPar).maximum_absolute_value() > 0.0 &&
883	// pp->type == cmbParam::offACSat &&
884	// pp->prn == prn) {
885	// AA(nObs+iCond, iPar) = 1.0;
886	// }
887	// }
888	// }
889	// }
890	}
891
892	return success;
893	}
894
895	// Process Epoch - Single-Epoch Method
896	////////////////////////////////////////////////////////////////////////////
897	t_irc bncComb::processEpoch_singleEpoch(QTextStream& out,
898	QMap<QString, cmbCorr*>& resCorr,
899	ColumnVector& dx) {
900
901	// Check Satellite Positions for Outliers
902	// --------------------------------------
903	if (checkOrbits(out) != success) {
904	return failure;
905	}
906
907	// Outlier Detection Loop
908	// ----------------------
909	while (true) {
910
911	// Remove Satellites that are not in Master
912	// ----------------------------------------
913	QMutableVectorIterator<cmbCorr*> it(corrs());
914	while (it.hasNext()) {
915	cmbCorr* corr = it.next();
916	QString prn = corr->_prn;
917	bool foundMaster = false;
918	QVectorIterator<cmbCorr*> itHlp(corrs());
919	while (itHlp.hasNext()) {
920	cmbCorr* corrHlp = itHlp.next();
921	QString prnHlp = corrHlp->_prn;
922	QString ACHlp = corrHlp->_acName;
923	if (ACHlp == _masterOrbitAC && prn == prnHlp) {
924	foundMaster = true;
925	break;
926	}
927	}
928	if (!foundMaster) {
929	delete corr;
930	it.remove();
931	}
932	}
933
934	// Count Number of Observations per Satellite and per AC
935	// -----------------------------------------------------
936	QMap<QString, int> numObsPrn;
937	QMap<QString, int> numObsAC;
938	QVectorIterator<cmbCorr*> itCorr(corrs());
939	while (itCorr.hasNext()) {
940	cmbCorr* corr = itCorr.next();
941	QString prn = corr->_prn;
942	QString AC = corr->_acName;
943	if (numObsPrn.find(prn) == numObsPrn.end()) {
944	numObsPrn[prn] = 1;
945	}
946	else {
947	numObsPrn[prn] += 1;
948	}
949	if (numObsAC.find(AC) == numObsAC.end()) {
950	numObsAC[AC] = 1;
951	}
952	else {
953	numObsAC[AC] += 1;
954	}
955	}
956
957	// Clean-Up the Paramters
958	// ----------------------
959	for (int iPar = 1; iPar <= _params.size(); iPar++) {
960	delete _params[iPar-1];
961	}
962	_params.clear();
963
964	// Set new Parameters
965	// ------------------
966	int nextPar = 0;
967
968	QMapIterator<QString, int> itAC(numObsAC);
969	while (itAC.hasNext()) {
970	itAC.next();
971	const QString& AC = itAC.key();
972	int numObs = itAC.value();
973	if (AC != _masterOrbitAC && numObs > 0) {
974	_params.push_back(new cmbParam(cmbParam::offACgps, ++nextPar, AC, ""));
975	if (_useGlonass) {
976	_params.push_back(new cmbParam(cmbParam::offACglo, ++nextPar, AC, ""));
977	}
978	}
979	}
980
981	QMapIterator<QString, int> itPrn(numObsPrn);
982	while (itPrn.hasNext()) {
983	itPrn.next();
984	const QString& prn = itPrn.key();
985	int numObs = itPrn.value();
986	if (numObs > 0) {
987	_params.push_back(new cmbParam(cmbParam::clkSat, ++nextPar, "", prn));
988	}
989	}
990
991	int nPar = _params.size();
992	ColumnVector x0(nPar);
993	x0 = 0.0;
994
995	// Create First-Design Matrix
996	// --------------------------
997	Matrix AA;
998	ColumnVector ll;
999	DiagonalMatrix PP;
1000	if (createAmat(AA, ll, PP, x0, resCorr) != success) {
1001	return failure;
1002	}
1003
1004	ColumnVector vv;
1005	try {
1006	Matrix ATP = AA.t() * PP;
1007	SymmetricMatrix NN; NN << ATP * AA;
1008	ColumnVector bb = ATP * ll;
1009	_QQ = NN.i();
1010	dx = _QQ * bb;
1011	vv = ll - AA * dx;
1012	}
1013	catch (Exception& exc) {
1014	out << exc.what() << endl;
1015	return failure;
1016	}
1017
1018	int maxResIndex;
1019	double maxRes = vv.maximum_absolute_value1(maxResIndex);
1020	out.setRealNumberNotation(QTextStream::FixedNotation);
1021	out.setRealNumberPrecision(3);
1022	out << _resTime.datestr().c_str() << " " << _resTime.timestr().c_str()
1023	<< " Maximum Residuum " << maxRes << ' '
1024	<< corrs()[maxResIndex-1]->_acName << ' ' << corrs()[maxResIndex-1]->_prn.mid(0,3);
1025
1026	if (maxRes > _MAXRES) {
1027	out << " Outlier" << endl;
1028	delete corrs()[maxResIndex-1];
1029	corrs().remove(maxResIndex-1);
1030	}
1031	else {
1032	out << " OK" << endl;
1033	out.setRealNumberNotation(QTextStream::FixedNotation);
1034	out.setRealNumberPrecision(3);
1035	for (int ii = 0; ii < vv.Nrows(); ii++) {
1036	const cmbCorr* corr = corrs()[ii];
1037	out << _resTime.datestr().c_str() << ' '
1038	<< _resTime.timestr().c_str() << " "
1039	<< corr->_acName << ' ' << corr->_prn.mid(0,3);
1040	out.setFieldWidth(6);
1041	out << " res = " << vv[ii] << endl;
1042	out.setFieldWidth(0);
1043	}
1044	return success;
1045	}
1046
1047	}
1048
1049	return failure;
1050	}
1051
1052	// Check Satellite Positions for Outliers
1053	////////////////////////////////////////////////////////////////////////////
1054	t_irc bncComb::checkOrbits(QTextStream& out) {
1055
1056	const double MAX_DISPLACEMENT = 0.20;
1057
1058	// Switch to last ephemeris (if possible)
1059	// --------------------------------------
1060	QMutableVectorIterator<cmbCorr*> im(corrs());
1061	while (im.hasNext()) {
1062	cmbCorr* corr = im.next();
1063	QString prn = corr->_prn;
1064
1065	t_eph* ephLast = _ephUser.ephLast(prn);
1066	t_eph* ephPrev = _ephUser.ephPrev(prn);
1067
1068	if (ephLast == 0) {
1069	out << "checkOrbit: missing eph (not found) " << corr->_prn.mid(0,3) << endl;
1070	delete corr;
1071	im.remove();
1072	}
1073	else if (corr->_eph == 0) {
1074	out << "checkOrbit: missing eph (zero) " << corr->_prn.mid(0,3) << endl;
1075	delete corr;
1076	im.remove();
1077	}
1078	else {
1079	if ( corr->_eph == ephLast \|\| corr->_eph == ephPrev ) {
1080	switchToLastEph(ephLast, corr);
1081	}
1082	else {
1083	out << "checkOrbit: missing eph (deleted) " << corr->_prn.mid(0,3) << endl;
1084	delete corr;
1085	im.remove();
1086	}
1087	}
1088	}
1089
1090	while (true) {
1091
1092	// Compute Mean Corrections for all Satellites
1093	// -------------------------------------------
1094	QMap<QString, int> numCorr;
1095	QMap<QString, ColumnVector> meanRao;
1096	QVectorIterator<cmbCorr*> it(corrs());
1097	while (it.hasNext()) {
1098	cmbCorr* corr = it.next();
1099	QString prn = corr->_prn;
1100	if (meanRao.find(prn) == meanRao.end()) {
1101	meanRao[prn].ReSize(4);
1102	meanRao[prn].Rows(1,3) = corr->_orbCorr._xr;
1103	meanRao[prn](4) = 1;
1104	}
1105	else {
1106	meanRao[prn].Rows(1,3) += corr->_orbCorr._xr;
1107	meanRao[prn](4) += 1;
1108	}
1109	if (numCorr.find(prn) == numCorr.end()) {
1110	numCorr[prn] = 1;
1111	}
1112	else {
1113	numCorr[prn] += 1;
1114	}
1115	}
1116
1117	// Compute Differences wrt Mean, find Maximum
1118	// ------------------------------------------
1119	QMap<QString, cmbCorr*> maxDiff;
1120	it.toFront();
1121	while (it.hasNext()) {
1122	cmbCorr* corr = it.next();
1123	QString prn = corr->_prn;
1124	if (meanRao[prn](4) != 0) {
1125	meanRao[prn] /= meanRao[prn](4);
1126	meanRao[prn](4) = 0;
1127	}
1128	corr->_diffRao = corr->_orbCorr._xr - meanRao[prn].Rows(1,3);
1129	if (maxDiff.find(prn) == maxDiff.end()) {
1130	maxDiff[prn] = corr;
1131	}
1132	else {
1133	double normMax = maxDiff[prn]->_diffRao.norm_Frobenius();
1134	double norm = corr->_diffRao.norm_Frobenius();
1135	if (norm > normMax) {
1136	maxDiff[prn] = corr;
1137	}
1138	}
1139	}
1140
1141	if (_ACs.size() == 1) {
1142	break;
1143	}
1144
1145	// Remove Outliers
1146	// ---------------
1147	bool removed = false;
1148	QMutableVectorIterator<cmbCorr*> im(corrs());
1149	while (im.hasNext()) {
1150	cmbCorr* corr = im.next();
1151	QString prn = corr->_prn;
1152	if (numCorr[prn] < 2) {
1153	delete corr;
1154	im.remove();
1155	}
1156	else if (corr == maxDiff[prn]) {
1157	double norm = corr->_diffRao.norm_Frobenius();
1158	if (norm > MAX_DISPLACEMENT) {
1159	out << _resTime.datestr().c_str() << " "
1160	<< _resTime.timestr().c_str() << " "
1161	<< "Orbit Outlier: "
1162	<< corr->_acName.toAscii().data() << " "
1163	<< prn.mid(0,3).toAscii().data() << " "
1164	<< corr->_iod << " "
1165	<< norm << endl;
1166	delete corr;
1167	im.remove();
1168	removed = true;
1169	}
1170	}
1171	}
1172
1173	if (!removed) {
1174	break;
1175	}
1176	}
1177
1178	return success;
1179	}
1180
1181	//
1182	////////////////////////////////////////////////////////////////////////////
1183	void bncComb::slotProviderIDChanged(QString mountPoint) {
1184	QMutexLocker locker(&_mutex);
1185
1186	// Find the AC Name
1187	// ----------------
1188	QString acName;
1189	QListIterator<cmbAC*> icAC(_ACs);
1190	while (icAC.hasNext()) {
1191	cmbAC* AC = icAC.next();
1192	if (AC->mountPoint == mountPoint) {
1193	acName = AC->name;
1194	break;
1195	}
1196	}
1197	if (acName.isEmpty()) {
1198	return;
1199	}
1200
1201	// Remove all corrections of the corresponding AC
1202	// ----------------------------------------------
1203	QListIterator<bncTime> itTime(_buffer.keys());
1204	while (itTime.hasNext()) {
1205	bncTime epoTime = itTime.next();
1206	QVector<cmbCorr*>& corrVec = _buffer[epoTime].corrs;
1207	QMutableVectorIterator<cmbCorr*> it(corrVec);
1208	while (it.hasNext()) {
1209	cmbCorr* corr = it.next();
1210	if (acName == corr->_acName) {
1211	delete corr;
1212	it.remove();
1213	}
1214	}
1215	}
1216
1217	// Reset Satellite Offsets
1218	// -----------------------
1219	if (_method == filter) {
1220	for (int iPar = 1; iPar <= _params.size(); iPar++) {
1221	cmbParam* pp = _params[iPar-1];
1222	if (pp->AC == acName && pp->type == cmbParam::offACSat) {
1223	pp->xx = 0.0;
1224	_QQ.Row(iPar) = 0.0;
1225	_QQ.Column(iPar) = 0.0;
1226	_QQ(iPar,iPar) = pp->sig0 * pp->sig0;
1227	}
1228	}
1229	}
1230	}

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

Download in other formats: