Context Navigation

bnccomb.cpp@ 10373

Last change on this file since 10373 was 3571, checked in by mervart, 13 years ago

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

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

Context Navigation

source: ntrip/branches/BNC_LM/combination/bnccomb.cpp@ 10373

Download in other formats: