Context Navigation

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

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

File size: 32.2 KB

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

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

Download in other formats: