Context Navigation

source: ntrip/trunk/BNC/combination/bnccomb.cpp@ 3555

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

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

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

Download in other formats: