Context Navigation

bnccomb.cpp@ 4207

Last change on this file since 4207 was 4183, checked in by mervart, 13 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_GPS = 32;
40	const int MAXPRN_GLONASS = 24;
41
42	using namespace std;
43
44	// Constructor
45	////////////////////////////////////////////////////////////////////////////
46	cmbParam::cmbParam(parType type_, int index_,
47	const QString& ac_, const QString& prn_) {
48
49	type = type_;
50	index = index_;
51	AC = ac_;
52	prn = prn_;
53	xx = 0.0;
54	eph = 0;
55
56	if (type == offACgps) {
57	epoSpec = true;
58	sig0 = sig0_offAC;
59	sigP = sig0;
60	}
61	else if (type == offACglo) {
62	epoSpec = true;
63	sig0 = sig0_offAC;
64	sigP = sig0;
65	}
66	else if (type == offACSat) {
67	epoSpec = false;
68	sig0 = sig0_offACSat;
69	sigP = sigP_offACSat;
70	}
71	else if (type == clkSat) {
72	epoSpec = true;
73	sig0 = sig0_clkSat;
74	sigP = sig0;
75	}
76	}
77
78	// Destructor
79	////////////////////////////////////////////////////////////////////////////
80	cmbParam::~cmbParam() {
81	}
82
83	// Partial
84	////////////////////////////////////////////////////////////////////////////
85	double cmbParam::partial(const QString& AC_, const QString& prn_) {
86
87	if (type == offACgps) {
88	if (AC == AC_ && prn_[0] == 'G') {
89	return 1.0;
90	}
91	}
92	else if (type == offACglo) {
93	if (AC == AC_ && prn_[0] == 'R') {
94	return 1.0;
95	}
96	}
97	else if (type == offACSat) {
98	if (AC == AC_ && prn == prn_) {
99	return 1.0;
100	}
101	}
102	else if (type == clkSat) {
103	if (prn == prn_) {
104	return 1.0;
105	}
106	}
107
108	return 0.0;
109	}
110
111	//
112	////////////////////////////////////////////////////////////////////////////
113	QString cmbParam::toString() const {
114
115	QString outStr;
116
117	if (type == offACgps) {
118	outStr = "AC offset GPS " + AC;
119	}
120	else if (type == offACglo) {
121	outStr = "AC offset GLO " + AC;
122	}
123	else if (type == offACSat) {
124	outStr = "Sat Offset " + AC + " " + prn;
125	}
126	else if (type == clkSat) {
127	outStr = "Clk Corr " + prn;
128	}
129
130	return outStr;
131	}
132
133	// Constructor
134	////////////////////////////////////////////////////////////////////////////
135	bncComb::bncComb() {
136
137	bncSettings settings;
138
139	QStringList combineStreams = settings.value("combineStreams").toStringList();
140
141	_cmbSampl = settings.value("cmbSampl").toInt();
142	if (_cmbSampl <= 0) {
143	_cmbSampl = 10;
144	}
145
146	_masterMissingEpochs = 0;
147
148	if (combineStreams.size() >= 1 && !combineStreams[0].isEmpty()) {
149	QListIterator<QString> it(combineStreams);
150	while (it.hasNext()) {
151	QStringList hlp = it.next().split(" ");
152	cmbAC* newAC = new cmbAC();
153	newAC->mountPoint = hlp[0];
154	newAC->name = hlp[1];
155	newAC->weight = hlp[2].toDouble();
156	if (_masterOrbitAC.isEmpty()) {
157	_masterOrbitAC = newAC->name;
158	}
159	_ACs.append(newAC);
160	}
161	}
162
163	_rtnetDecoder = 0;
164
165	connect(this, SIGNAL(newMessage(QByteArray,bool)),
166	((bncApp*)qApp), SLOT(slotMessage(const QByteArray,bool)));
167
168	// Combination Method
169	// ------------------
170	if (settings.value("cmbMethod").toString() == "Single-Epoch") {
171	_method = singleEpoch;
172	}
173	else {
174	_method = filter;
175	}
176
177	// Use Glonass
178	// -----------
179	if ( Qt::CheckState(settings.value("pppGLONASS").toInt()) == Qt::Checked) {
180	_useGlonass = true;
181	}
182	else {
183	_useGlonass = false;
184	}
185
186	// Initialize Parameters (model: Clk_Corr = AC_Offset + Sat_Offset + Clk)
187	// ----------------------------------------------------------------------
188	if (_method == filter) {
189	int nextPar = 0;
190	QListIterator<cmbAC*> it(_ACs);
191	while (it.hasNext()) {
192	cmbAC* AC = it.next();
193	_params.push_back(new cmbParam(cmbParam::offACgps, ++nextPar, AC->name, ""));
194	for (int iGps = 1; iGps <= MAXPRN_GPS; iGps++) {
195	QString prn = QString("G%1").arg(iGps, 2, 10, QChar('0'));
196	_params.push_back(new cmbParam(cmbParam::offACSat, ++nextPar,
197	AC->name, prn));
198	}
199	if (_useGlonass) {
200	_params.push_back(new cmbParam(cmbParam::offACglo, ++nextPar, AC->name, ""));
201	for (int iGlo = 1; iGlo <= MAXPRN_GLONASS; iGlo++) {
202	QString prn = QString("R%1").arg(iGlo, 2, 10, QChar('0'));
203	_params.push_back(new cmbParam(cmbParam::offACSat, ++nextPar,
204	AC->name, prn));
205	}
206	}
207	}
208	for (int iGps = 1; iGps <= MAXPRN_GPS; iGps++) {
209	QString prn = QString("G%1").arg(iGps, 2, 10, QChar('0'));
210	_params.push_back(new cmbParam(cmbParam::clkSat, ++nextPar, "", prn));
211	}
212	if (_useGlonass) {
213	for (int iGlo = 1; iGlo <= MAXPRN_GLONASS; iGlo++) {
214	QString prn = QString("R%1").arg(iGlo, 2, 10, QChar('0'));
215	_params.push_back(new cmbParam(cmbParam::clkSat, ++nextPar, "", prn));
216	}
217	}
218
219	// Initialize Variance-Covariance Matrix
220	// -------------------------------------
221	_QQ.ReSize(_params.size());
222	_QQ = 0.0;
223	for (int iPar = 1; iPar <= _params.size(); iPar++) {
224	cmbParam* pp = _params[iPar-1];
225	_QQ(iPar,iPar) = pp->sig0 * pp->sig0;
226	}
227	}
228
229	// ANTEX File
230	// ----------
231	_antex = 0;
232	QString antexFileName = settings.value("pppAntex").toString();
233	if (!antexFileName.isEmpty()) {
234	_antex = new bncAntex();
235	if (_antex->readFile(antexFileName) != success) {
236	emit newMessage("wrong ANTEX file", true);
237	delete _antex;
238	_antex = 0;
239	}
240	}
241
242	// Maximal Residuum
243	// ----------------
244	_MAXRES = settings.value("cmbMaxres").toDouble();
245	if (_MAXRES <= 0.0) {
246	_MAXRES = 999.0;
247	}
248	}
249
250	// Destructor
251	////////////////////////////////////////////////////////////////////////////
252	bncComb::~bncComb() {
253	QListIterator<cmbAC*> icAC(_ACs);
254	while (icAC.hasNext()) {
255	delete icAC.next();
256	}
257	delete _rtnetDecoder;
258	delete _antex;
259	for (int iPar = 1; iPar <= _params.size(); iPar++) {
260	delete _params[iPar-1];
261	}
262	QListIterator<bncTime> itTime(_buffer.keys());
263	while (itTime.hasNext()) {
264	bncTime epoTime = itTime.next();
265	_buffer.remove(epoTime);
266	}
267	QMapIterator<QString, cmbCorr*> itOrbCorr(_orbitCorrs);
268	while (itOrbCorr.hasNext()) {
269	itOrbCorr.next();
270	delete itOrbCorr.value();
271	}
272	}
273
274	// Read and store one correction line
275	////////////////////////////////////////////////////////////////////////////
276	void bncComb::processCorrLine(const QString& staID, const QString& line) {
277	QMutexLocker locker(&_mutex);
278
279	// Find the AC Name
280	// ----------------
281	QString acName;
282	QListIterator<cmbAC*> icAC(_ACs);
283	while (icAC.hasNext()) {
284	cmbAC* AC = icAC.next();
285	if (AC->mountPoint == staID) {
286	acName = AC->name;
287	break;
288	}
289	}
290	if (acName.isEmpty()) {
291	return;
292	}
293
294	// Read the Correction
295	// -------------------
296	cmbCorr* newCorr = new cmbCorr();
297	newCorr->acName = acName;
298	if (!newCorr->readLine(line) == success) {
299	delete newCorr;
300	return;
301	}
302
303	// Check Glonass
304	// -------------
305	if (!_useGlonass) {
306	if (newCorr->prn[0] == 'R') {
307	delete newCorr;
308	return;
309	}
310	}
311
312	// Save Orbit-Only Corrections
313	// ---------------------------
314	if (newCorr->tRao.valid() && !newCorr->tClk.valid()) {
315	QString corrID = newCorr->ID();
316	if (_orbitCorrs.find(corrID) != _orbitCorrs.end()) {
317	delete _orbitCorrs[corrID];
318	}
319	_orbitCorrs[corrID] = newCorr;
320	return;
321	}
322
323	// Merge with saved orbit correction
324	// ---------------------------------
325	else if (newCorr->tClk.valid() && !newCorr->tRao.valid()) {
326	QString corrID = newCorr->ID();
327	if (_orbitCorrs.find(corrID) != _orbitCorrs.end()) {
328	mergeOrbitCorr(_orbitCorrs[corrID], newCorr);
329	}
330	}
331
332	// Check Modulo Time
333	// -----------------
334	if (int(newCorr->tClk.gpssec()) % _cmbSampl != 0.0) {
335	delete newCorr;
336	return;
337	}
338
339	// Delete old corrections
340	// ----------------------
341	if (_resTime.valid() && newCorr->tClk <= _resTime) {
342	delete newCorr;
343	return;
344	}
345
346	// Check the Ephemeris
347	//--------------------
348	if (_eph.find(newCorr->prn) == _eph.end()) {
349	delete newCorr;
350	return;
351	}
352	else {
353	t_eph* lastEph = _eph[newCorr->prn]->last;
354	t_eph* prevEph = _eph[newCorr->prn]->prev;
355	if (lastEph && lastEph->IOD() == newCorr->iod) {
356	newCorr->eph = lastEph;
357	}
358	else if (lastEph && prevEph && prevEph->IOD() == newCorr->iod) {
359	newCorr->eph = prevEph;
360	switchToLastEph(lastEph, newCorr);
361	}
362	else {
363	delete newCorr;
364	return;
365	}
366	}
367
368	// Process previous Epoch(s)
369	// -------------------------
370	QListIterator<bncTime> itTime(_buffer.keys());
371	while (itTime.hasNext()) {
372	bncTime epoTime = itTime.next();
373	if (epoTime < newCorr->tClk - _cmbSampl) {
374	_resTime = epoTime;
375	processEpoch();
376	}
377	}
378
379	// Merge or add the correction
380	// ---------------------------
381	QVector<cmbCorr*>& corrs = _buffer[newCorr->tClk].corrs;
382	cmbCorr* existingCorr = 0;
383	QVectorIterator<cmbCorr*> itCorr(corrs);
384	while (itCorr.hasNext()) {
385	cmbCorr* hlp = itCorr.next();
386	if (hlp->prn == newCorr->prn && hlp->acName == newCorr->acName) {
387	existingCorr = hlp;
388	break;
389	}
390	}
391	if (existingCorr) {
392	delete newCorr; newCorr = 0;
393	existingCorr->readLine(line); // merge (multiple messages)
394
395	}
396	else {
397	corrs.append(newCorr);
398	}
399	}
400
401	// Change the correction so that it refers to last received ephemeris
402	////////////////////////////////////////////////////////////////////////////
403	void bncComb::switchToLastEph(const t_eph* lastEph, t_corr* corr) {
404
405	if (corr->eph == lastEph) {
406	return;
407	}
408
409	ColumnVector oldXC(4);
410	ColumnVector oldVV(3);
411	corr->eph->position(corr->tRao.gpsw(), corr->tRao.gpssec(),
412	oldXC.data(), oldVV.data());
413
414	ColumnVector newXC(4);
415	ColumnVector newVV(3);
416	lastEph->position(corr->tRao.gpsw(), corr->tRao.gpssec(),
417	newXC.data(), newVV.data());
418
419	ColumnVector dX = newXC.Rows(1,3) - oldXC.Rows(1,3);
420	ColumnVector dV = newVV - oldVV;
421	double dC = newXC(4) - oldXC(4);
422
423	ColumnVector dRAO(3);
424	XYZ_to_RSW(newXC.Rows(1,3), newVV, dX, dRAO);
425
426	ColumnVector dDotRAO(3);
427	XYZ_to_RSW(newXC.Rows(1,3), newVV, dV, dDotRAO);
428
429	QString msg = "switch corr " + corr->prn
430	+ QString(" %1 -> %2 %3").arg(corr->iod,3)
431	.arg(lastEph->IOD(),3).arg(dC*t_CST::c, 8, 'f', 4);
432
433	emit newMessage(msg.toAscii(), false);
434
435	corr->iod = lastEph->IOD();
436	corr->eph = lastEph;
437	corr->rao += dRAO;
438	corr->dotRao += dDotRAO;
439	corr->dClk -= dC;
440	}
441
442	// Process Epoch
443	////////////////////////////////////////////////////////////////////////////
444	void bncComb::processEpoch() {
445
446	_log.clear();
447
448	QTextStream out(&_log, QIODevice::WriteOnly);
449
450	out << endl << "Combination:" << endl
451	<< "------------------------------" << endl;
452
453	// Observation Statistics
454	// ----------------------
455	bool masterPresent = false;
456	QListIterator<cmbAC*> icAC(_ACs);
457	while (icAC.hasNext()) {
458	cmbAC* AC = icAC.next();
459	AC->numObs = 0;
460	QVectorIterator<cmbCorr*> itCorr(corrs());
461	while (itCorr.hasNext()) {
462	cmbCorr* corr = itCorr.next();
463	if (corr->acName == AC->name) {
464	AC->numObs += 1;
465	if (AC->name == _masterOrbitAC) {
466	masterPresent = true;
467	}
468	}
469	}
470	out << AC->name.toAscii().data() << ": " << AC->numObs << endl;
471	}
472
473	// If Master not present, switch to another one
474	// --------------------------------------------
475	if (masterPresent) {
476	_masterMissingEpochs = 0;
477	}
478	else {
479	++_masterMissingEpochs;
480	if (_masterMissingEpochs < 10) {
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.

Context Navigation

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

Download in other formats: