Context Navigation

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

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

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

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

Download in other formats: