Context Navigation

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

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

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

Download in other formats: