Context Navigation

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

Last change on this file since 4974 was 4889, checked in by mervart, 12 years ago

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

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

Download in other formats: