Context Navigation

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

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

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

Download in other formats: