source: ntrip/trunk/BNC/src/PPP_SSR_I/pppFilter.cpp@ 8253

Last change on this file since 8253 was 8253, checked in by stoecker, 6 years ago

fix include incompatibilities
  • Property svn:keywords set to Author Date Id Rev URL;svn:eol-style=native
  • Property svn:mime-type set to text/plain
File size: 38.7 KB
Line 
1// Part of BNC, a utility for retrieving decoding and
2// converting GNSS data streams from NTRIP broadcasters.
3//
4// Copyright (C) 2007
5// German Federal Agency for Cartography and Geodesy (BKG)
6// http://www.bkg.bund.de
7// Czech Technical University Prague, Department of Geodesy
8// http://www.fsv.cvut.cz
9//
10// Email: euref-ip@bkg.bund.de
11//
12// This program is free software; you can redistribute it and/or
13// modify it under the terms of the GNU General Public License
14// as published by the Free Software Foundation, version 2.
15//
16// This program is distributed in the hope that it will be useful,
17// but WITHOUT ANY WARRANTY; without even the implied warranty of
18// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19// GNU General Public License for more details.
20//
21// You should have received a copy of the GNU General Public License
22// along with this program; if not, write to the Free Software
23// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
24
25/* -------------------------------------------------------------------------
26 * BKG NTRIP Client
27 * -------------------------------------------------------------------------
28 *
29 * Class: t_pppParam, t_pppFilter
30 *
31 * Purpose: Model for PPP
32 *
33 * Author: L. Mervart
34 *
35 * Created: 01-Dec-2009
36 *
37 * Changes:
38 *
39 * -----------------------------------------------------------------------*/
40
41#include <iomanip>
42#include <cmath>
43#include <sstream>
44#include <newmatio.h>
45#include <newmatap.h>
46
47#include "pppFilter.h"
48#include "pppClient.h"
49#include "bncutils.h"
50#include "bncantex.h"
51#include "pppOptions.h"
52#include "pppModel.h"
53
54using namespace BNC_PPP;
55using namespace std;
56
57const double MAXRES_CODE = 2.98 * 3.0;
58const double MAXRES_PHASE_GPS = 0.04;
59const double MAXRES_PHASE_GLONASS = 2.98 * 0.03;
60const double GLONASS_WEIGHT_FACTOR = 5.0;
61const double BDS_WEIGHT_FACTOR = 2.0;
62
63#define LOG (_pppClient->log())
64#define OPT (_pppClient->opt())
65
66// Constructor
67////////////////////////////////////////////////////////////////////////////
68t_pppParam::t_pppParam(t_pppParam::parType typeIn, int indexIn,
69 const QString& prnIn) {
70 type = typeIn;
71 index = indexIn;
72 prn = prnIn;
73 index_old = 0;
74 xx = 0.0;
75 numEpo = 0;
76}
77
78// Destructor
79////////////////////////////////////////////////////////////////////////////
80t_pppParam::~t_pppParam() {
81}
82
83// Partial
84////////////////////////////////////////////////////////////////////////////
85double t_pppParam::partial(t_satData* satData, bool phase) {
86
87 Tracer tracer("t_pppParam::partial");
88
89 // Coordinates
90 // -----------
91 if (type == CRD_X) {
92 return (xx - satData->xx(1)) / satData->rho;
93 }
94 else if (type == CRD_Y) {
95 return (xx - satData->xx(2)) / satData->rho;
96 }
97 else if (type == CRD_Z) {
98 return (xx - satData->xx(3)) / satData->rho;
99 }
100
101 // Receiver Clocks
102 // ---------------
103 else if (type == RECCLK) {
104 return 1.0;
105 }
106
107 // Troposphere
108 // -----------
109 else if (type == TROPO) {
110 return 1.0 / sin(satData->eleSat);
111 }
112
113 // Glonass Offset
114 // --------------
115 else if (type == GLONASS_OFFSET) {
116 if (satData->prn[0] == 'R') {
117 return 1.0;
118 }
119 else {
120 return 0.0;
121 }
122 }
123
124 // Galileo Offset
125 // --------------
126 else if (type == GALILEO_OFFSET) {
127 if (satData->prn[0] == 'E') {
128 return 1.0;
129 }
130 else {
131 return 0.0;
132 }
133 }
134
135 // BDS Offset
136 // ----------
137 else if (type == BDS_OFFSET) {
138 if (satData->prn[0] == 'C') {
139 return 1.0;
140 }
141 else {
142 return 0.0;
143 }
144 }
145
146 // Ambiguities
147 // -----------
148 else if (type == AMB_L3) {
149 if (phase && satData->prn == prn) {
150 return 1.0;
151 }
152 else {
153 return 0.0;
154 }
155 }
156
157 // Default return
158 // --------------
159 return 0.0;
160}
161
162// Constructor
163////////////////////////////////////////////////////////////////////////////
164t_pppFilter::t_pppFilter(t_pppClient* pppClient) {
165
166 _pppClient = pppClient;
167 _tides = new t_tides();
168
169 // Antenna Name, ANTEX File
170 // ------------------------
171 _antex = 0;
172 if (!OPT->_antexFileName.empty()) {
173 _antex = new bncAntex(OPT->_antexFileName.c_str());
174 }
175
176 // Bancroft Coordinates
177 // --------------------
178 _xcBanc.ReSize(4); _xcBanc = 0.0;
179 _ellBanc.ReSize(3); _ellBanc = 0.0;
180
181 // Save copy of data (used in outlier detection)
182 // ---------------------------------------------
183 _epoData_sav = new t_epoData();
184
185 // Some statistics
186 // ---------------
187 _neu.ReSize(3); _neu = 0.0;
188 _numSat = 0;
189 _hDop = 0.0;
190}
191
192// Destructor
193////////////////////////////////////////////////////////////////////////////
194t_pppFilter::~t_pppFilter() {
195 delete _tides;
196 delete _antex;
197 for (int iPar = 1; iPar <= _params.size(); iPar++) {
198 delete _params[iPar-1];
199 }
200 for (int iPar = 1; iPar <= _params_sav.size(); iPar++) {
201 delete _params_sav[iPar-1];
202 }
203 delete _epoData_sav;
204}
205
206// Reset Parameters and Variance-Covariance Matrix
207////////////////////////////////////////////////////////////////////////////
208void t_pppFilter::reset() {
209
210 Tracer tracer("t_pppFilter::reset");
211
212 double lastTrp = 0.0;
213 for (int ii = 0; ii < _params.size(); ii++) {
214 t_pppParam* pp = _params[ii];
215 if (pp->type == t_pppParam::TROPO) {
216 lastTrp = pp->xx;
217 }
218 delete pp;
219 }
220 _params.clear();
221
222 int nextPar = 0;
223 _params.push_back(new t_pppParam(t_pppParam::CRD_X, ++nextPar, ""));
224 _params.push_back(new t_pppParam(t_pppParam::CRD_Y, ++nextPar, ""));
225 _params.push_back(new t_pppParam(t_pppParam::CRD_Z, ++nextPar, ""));
226 _params.push_back(new t_pppParam(t_pppParam::RECCLK, ++nextPar, ""));
227 if (OPT->estTrp()) {
228 _params.push_back(new t_pppParam(t_pppParam::TROPO, ++nextPar, ""));
229 }
230 if (OPT->useSystem('R')) {
231 _params.push_back(new t_pppParam(t_pppParam::GLONASS_OFFSET, ++nextPar, ""));
232 }
233 if (OPT->useSystem('E')) {
234 _params.push_back(new t_pppParam(t_pppParam::GALILEO_OFFSET, ++nextPar, ""));
235 }
236 if (OPT->useSystem('C')) {
237 _params.push_back(new t_pppParam(t_pppParam::BDS_OFFSET, ++nextPar, ""));
238 }
239
240 _QQ.ReSize(_params.size());
241 _QQ = 0.0;
242 for (int iPar = 1; iPar <= _params.size(); iPar++) {
243 t_pppParam* pp = _params[iPar-1];
244 pp->xx = 0.0;
245 if (pp->isCrd()) {
246 _QQ(iPar,iPar) = OPT->_aprSigCrd(1) * OPT->_aprSigCrd(1);
247 }
248 else if (pp->type == t_pppParam::RECCLK) {
249 _QQ(iPar,iPar) = OPT->_noiseClk * OPT->_noiseClk;
250 }
251 else if (pp->type == t_pppParam::TROPO) {
252 _QQ(iPar,iPar) = OPT->_aprSigTrp * OPT->_aprSigTrp;
253 pp->xx = lastTrp;
254 }
255 else if (pp->type == t_pppParam::GLONASS_OFFSET) {
256 _QQ(iPar,iPar) = 1000.0 * 1000.0;
257 }
258 else if (pp->type == t_pppParam::GALILEO_OFFSET) {
259 _QQ(iPar,iPar) = 1000.0 * 1000.0;
260 }
261 else if (pp->type == t_pppParam::BDS_OFFSET) {
262 _QQ(iPar,iPar) = 1000.0 * 1000.0;
263 }
264 }
265}
266
267// Bancroft Solution
268////////////////////////////////////////////////////////////////////////////
269t_irc t_pppFilter::cmpBancroft(t_epoData* epoData) {
270
271 Tracer tracer("t_pppFilter::cmpBancroft");
272
273 if (int(epoData->sizeSys('G')) < OPT->_minObs) {
274 LOG << "t_pppFilter::cmpBancroft: not enough data\n";
275 return failure;
276 }
277
278 Matrix BB(epoData->sizeSys('G'), 4);
279
280 QMapIterator<QString, t_satData*> it(epoData->satData);
281 int iObsBanc = 0;
282 while (it.hasNext()) {
283 it.next();
284 t_satData* satData = it.value();
285 if (satData->system() == 'G') {
286 ++iObsBanc;
287 QString prn = it.key();
288 BB(iObsBanc, 1) = satData->xx(1);
289 BB(iObsBanc, 2) = satData->xx(2);
290 BB(iObsBanc, 3) = satData->xx(3);
291 BB(iObsBanc, 4) = satData->P3 + satData->clk;
292 }
293 }
294
295 bancroft(BB, _xcBanc);
296
297 if (std::isnan(_xcBanc(1)) ||
298 std::isnan(_xcBanc(2)) ||
299 std::isnan(_xcBanc(3))) {
300 return failure;
301 }
302
303 // Ellipsoidal Coordinates
304 // ------------------------
305 xyz2ell(_xcBanc.data(), _ellBanc.data());
306
307 // Compute Satellite Elevations
308 // ----------------------------
309 QMutableMapIterator<QString, t_satData*> im(epoData->satData);
310 while (im.hasNext()) {
311 im.next();
312 t_satData* satData = im.value();
313 cmpEle(satData);
314 if (satData->eleSat < OPT->_minEle) {
315 delete satData;
316 im.remove();
317 }
318 }
319
320 return success;
321}
322
323// Computed Value
324////////////////////////////////////////////////////////////////////////////
325double t_pppFilter::cmpValue(t_satData* satData, bool phase) {
326
327 Tracer tracer("t_pppFilter::cmpValue");
328
329 ColumnVector xRec(3);
330 xRec(1) = x();
331 xRec(2) = y();
332 xRec(3) = z();
333
334 double rho0 = (satData->xx - xRec).norm_Frobenius();
335 double dPhi = t_CST::omega * rho0 / t_CST::c;
336
337 xRec(1) = x() * cos(dPhi) - y() * sin(dPhi);
338 xRec(2) = y() * cos(dPhi) + x() * sin(dPhi);
339 xRec(3) = z();
340
341 xRec += _tides->displacement(_time, xRec);
342
343 satData->rho = (satData->xx - xRec).norm_Frobenius();
344
345 double tropDelay = delay_saast(satData->eleSat) +
346 trp() / sin(satData->eleSat);
347
348 double wind = 0.0;
349 if (phase) {
350 wind = windUp(satData->prn, satData->xx, xRec) * satData->lambda3;
351 }
352
353 double offset = 0.0;
354 t_frequency::type frqA = t_frequency::G1;
355 t_frequency::type frqB = t_frequency::G2;
356 if (satData->prn[0] == 'R') {
357 offset = Glonass_offset();
358 frqA = t_frequency::R1;
359 frqB = t_frequency::R2;
360 }
361 else if (satData->prn[0] == 'E') {
362 offset = Galileo_offset();
363 //frqA = t_frequency::E1; as soon as available
364 //frqB = t_frequency::E5; -"-
365 }
366 else if (satData->prn[0] == 'C') {
367 offset = Bds_offset();
368 //frqA = t_frequency::C2; as soon as available
369 //frqB = t_frequency::C7; -"-
370 }
371 double phaseCenter = 0.0;
372 if (_antex) {
373 bool found;
374 phaseCenter = satData->lkA * _antex->rcvCorr(OPT->_antNameRover, frqA,
375 satData->eleSat, satData->azSat,
376 found)
377 + satData->lkB * _antex->rcvCorr(OPT->_antNameRover, frqB,
378 satData->eleSat, satData->azSat,
379 found);
380 if (!found) {
381 LOG << "ANTEX: antenna >" << OPT->_antNameRover << "< not found\n";
382 }
383 }
384
385 double antennaOffset = 0.0;
386 double cosa = cos(satData->azSat);
387 double sina = sin(satData->azSat);
388 double cose = cos(satData->eleSat);
389 double sine = sin(satData->eleSat);
390 antennaOffset = -OPT->_neuEccRover(1) * cosa*cose
391 -OPT->_neuEccRover(2) * sina*cose
392 -OPT->_neuEccRover(3) * sine;
393
394 return satData->rho + phaseCenter + antennaOffset + clk()
395 + offset - satData->clk + tropDelay + wind;
396}
397
398// Tropospheric Model (Saastamoinen)
399////////////////////////////////////////////////////////////////////////////
400double t_pppFilter::delay_saast(double Ele) {
401
402 Tracer tracer("t_pppFilter::delay_saast");
403
404 double xyz[3];
405 xyz[0] = x();
406 xyz[1] = y();
407 xyz[2] = z();
408 double ell[3];
409 xyz2ell(xyz, ell);
410 double height = ell[2];
411
412 double pp = 1013.25 * pow(1.0 - 2.26e-5 * height, 5.225);
413 double TT = 18.0 - height * 0.0065 + 273.15;
414 double hh = 50.0 * exp(-6.396e-4 * height);
415 double ee = hh / 100.0 * exp(-37.2465 + 0.213166*TT - 0.000256908*TT*TT);
416
417 double h_km = height / 1000.0;
418
419 if (h_km < 0.0) h_km = 0.0;
420 if (h_km > 5.0) h_km = 5.0;
421 int ii = int(h_km + 1);
422 double href = ii - 1;
423
424 double bCor[6];
425 bCor[0] = 1.156;
426 bCor[1] = 1.006;
427 bCor[2] = 0.874;
428 bCor[3] = 0.757;
429 bCor[4] = 0.654;
430 bCor[5] = 0.563;
431
432 double BB = bCor[ii-1] + (bCor[ii]-bCor[ii-1]) * (h_km - href);
433
434 double zen = M_PI/2.0 - Ele;
435
436 return (0.002277/cos(zen)) * (pp + ((1255.0/TT)+0.05)*ee - BB*(tan(zen)*tan(zen)));
437}
438
439// Prediction Step of the Filter
440////////////////////////////////////////////////////////////////////////////
441void t_pppFilter::predict(int iPhase, t_epoData* epoData) {
442
443 Tracer tracer("t_pppFilter::predict");
444
445 if (iPhase == 0) {
446
447 const double maxSolGap = 60.0;
448
449 bool firstCrd = false;
450 if (!_lastTimeOK.valid() || (maxSolGap > 0.0 && _time - _lastTimeOK > maxSolGap)) {
451 firstCrd = true;
452 _startTime = epoData->tt;
453 reset();
454 }
455
456 // Use different white noise for Quick-Start mode
457 // ----------------------------------------------
458 double sigCrdP_used = OPT->_noiseCrd(1);
459 if ( OPT->_seedingTime > 0.0 && OPT->_seedingTime > (epoData->tt - _startTime) ) {
460 sigCrdP_used = 0.0;
461 }
462
463 // Predict Parameter values, add white noise
464 // -----------------------------------------
465 for (int iPar = 1; iPar <= _params.size(); iPar++) {
466 t_pppParam* pp = _params[iPar-1];
467
468 // Coordinates
469 // -----------
470 if (pp->type == t_pppParam::CRD_X) {
471 if (firstCrd) {
472 if (OPT->xyzAprRoverSet()) {
473 pp->xx = OPT->_xyzAprRover[0];
474 }
475 else {
476 pp->xx = _xcBanc(1);
477 }
478 }
479 _QQ(iPar,iPar) += sigCrdP_used * sigCrdP_used;
480 }
481 else if (pp->type == t_pppParam::CRD_Y) {
482 if (firstCrd) {
483 if (OPT->xyzAprRoverSet()) {
484 pp->xx = OPT->_xyzAprRover[1];
485 }
486 else {
487 pp->xx = _xcBanc(2);
488 }
489 }
490 _QQ(iPar,iPar) += sigCrdP_used * sigCrdP_used;
491 }
492 else if (pp->type == t_pppParam::CRD_Z) {
493 if (firstCrd) {
494 if (OPT->xyzAprRoverSet()) {
495 pp->xx = OPT->_xyzAprRover[2];
496 }
497 else {
498 pp->xx = _xcBanc(3);
499 }
500 }
501 _QQ(iPar,iPar) += sigCrdP_used * sigCrdP_used;
502 }
503
504 // Receiver Clocks
505 // ---------------
506 else if (pp->type == t_pppParam::RECCLK) {
507 pp->xx = _xcBanc(4);
508 for (int jj = 1; jj <= _params.size(); jj++) {
509 _QQ(iPar, jj) = 0.0;
510 }
511 _QQ(iPar,iPar) = OPT->_noiseClk * OPT->_noiseClk;
512 }
513
514 // Tropospheric Delay
515 // ------------------
516 else if (pp->type == t_pppParam::TROPO) {
517 _QQ(iPar,iPar) += OPT->_noiseTrp * OPT->_noiseTrp;
518 }
519
520 // Glonass Offset
521 // --------------
522 else if (pp->type == t_pppParam::GLONASS_OFFSET) {
523 pp->xx = 0.0;
524 for (int jj = 1; jj <= _params.size(); jj++) {
525 _QQ(iPar, jj) = 0.0;
526 }
527 _QQ(iPar,iPar) = 1000.0 * 1000.0;
528 }
529
530 // Galileo Offset
531 // --------------
532 else if (pp->type == t_pppParam::GALILEO_OFFSET) {
533 _QQ(iPar,iPar) += 0.1 * 0.1;
534 }
535
536 // BDS Offset
537 // ----------
538 else if (pp->type == t_pppParam::BDS_OFFSET) {
539 _QQ(iPar,iPar) += 0.1 * 0.1; //TODO: TEST
540 }
541 }
542 }
543
544 // Add New Ambiguities if necessary
545 // --------------------------------
546 if (OPT->ambLCs('G').size() || OPT->ambLCs('R').size() ||
547 OPT->ambLCs('E').size() || OPT->ambLCs('C').size()) {
548
549 // Make a copy of QQ and xx, set parameter indices
550 // -----------------------------------------------
551 SymmetricMatrix QQ_old = _QQ;
552
553 for (int iPar = 1; iPar <= _params.size(); iPar++) {
554 _params[iPar-1]->index_old = _params[iPar-1]->index;
555 _params[iPar-1]->index = 0;
556 }
557
558 // Remove Ambiguity Parameters without observations
559 // ------------------------------------------------
560 int iPar = 0;
561 QMutableVectorIterator<t_pppParam*> im(_params);
562 while (im.hasNext()) {
563 t_pppParam* par = im.next();
564 bool removed = false;
565 if (par->type == t_pppParam::AMB_L3) {
566 if (epoData->satData.find(par->prn) == epoData->satData.end()) {
567 removed = true;
568 delete par;
569 im.remove();
570 }
571 }
572 if (! removed) {
573 ++iPar;
574 par->index = iPar;
575 }
576 }
577
578 // Add new ambiguity parameters
579 // ----------------------------
580 QMapIterator<QString, t_satData*> it(epoData->satData);
581 while (it.hasNext()) {
582 it.next();
583 t_satData* satData = it.value();
584 addAmb(satData);
585 }
586
587 int nPar = _params.size();
588 _QQ.ReSize(nPar); _QQ = 0.0;
589 for (int i1 = 1; i1 <= nPar; i1++) {
590 t_pppParam* p1 = _params[i1-1];
591 if (p1->index_old != 0) {
592 _QQ(p1->index, p1->index) = QQ_old(p1->index_old, p1->index_old);
593 for (int i2 = 1; i2 <= nPar; i2++) {
594 t_pppParam* p2 = _params[i2-1];
595 if (p2->index_old != 0) {
596 _QQ(p1->index, p2->index) = QQ_old(p1->index_old, p2->index_old);
597 }
598 }
599 }
600 }
601
602 for (int ii = 1; ii <= nPar; ii++) {
603 t_pppParam* par = _params[ii-1];
604 if (par->index_old == 0) {
605 _QQ(par->index, par->index) = OPT->_aprSigAmb * OPT->_aprSigAmb;
606 }
607 par->index_old = par->index;
608 }
609 }
610}
611
612// Update Step of the Filter (currently just a single-epoch solution)
613////////////////////////////////////////////////////////////////////////////
614t_irc t_pppFilter::update(t_epoData* epoData) {
615
616 Tracer tracer("t_pppFilter::update");
617
618 _time = epoData->tt; // current epoch time
619
620 if (OPT->useOrbClkCorr()) {
621 LOG << "Precise Point Positioning of Epoch " << _time.datestr() << "_" << _time.timestr(3)
622 << "\n---------------------------------------------------------------\n";
623 }
624 else {
625 LOG << "Single Point Positioning of Epoch " << _time.datestr() << "_" << _time.timestr(3)
626 << "\n---------------------------------------------------------------\n";
627 }
628
629 // Outlier Detection Loop
630 // ----------------------
631 if (update_p(epoData) != success) {
632 return failure;
633 }
634
635 // Set Solution Vector
636 // -------------------
637 LOG.setf(ios::fixed);
638 QVectorIterator<t_pppParam*> itPar(_params);
639 while (itPar.hasNext()) {
640 t_pppParam* par = itPar.next();
641 if (par->type == t_pppParam::RECCLK) {
642 LOG << "\n" << _time.datestr() << "_" << _time.timestr(3)
643 << " CLK " << setw(10) << setprecision(3) << par->xx
644 << " +- " << setw(6) << setprecision(3)
645 << sqrt(_QQ(par->index,par->index));
646 }
647 else if (par->type == t_pppParam::AMB_L3) {
648 ++par->numEpo;
649 LOG << "\n" << _time.datestr() << "_" << _time.timestr(3)
650 << " AMB " << par->prn.mid(0,3).toLatin1().data() << " "
651 << setw(10) << setprecision(3) << par->xx
652 << " +- " << setw(6) << setprecision(3)
653 << sqrt(_QQ(par->index,par->index))
654 << " epo = " << par->numEpo;
655 }
656 else if (par->type == t_pppParam::TROPO) {
657 double aprTrp = delay_saast(M_PI/2.0);
658 LOG << "\n" << _time.datestr() << "_" << _time.timestr(3)
659 << " TRP " << par->prn.mid(0,3).toLatin1().data()
660 << setw(7) << setprecision(3) << aprTrp << " "
661 << setw(6) << setprecision(3) << showpos << par->xx << noshowpos
662 << " +- " << setw(6) << setprecision(3)
663 << sqrt(_QQ(par->index,par->index));
664 }
665 else if (par->type == t_pppParam::GLONASS_OFFSET) {
666 LOG << "\n" << _time.datestr() << "_" << _time.timestr(3)
667 << " OFFGLO " << setw(10) << setprecision(3) << par->xx
668 << " +- " << setw(6) << setprecision(3)
669 << sqrt(_QQ(par->index,par->index));
670 }
671 else if (par->type == t_pppParam::GALILEO_OFFSET) {
672 LOG << "\n" << _time.datestr() << "_" << _time.timestr(3)
673 << " OFFGAL " << setw(10) << setprecision(3) << par->xx
674 << " +- " << setw(6) << setprecision(3)
675 << sqrt(_QQ(par->index,par->index));
676 }
677 else if (par->type == t_pppParam::BDS_OFFSET) {
678 LOG << "\n" << _time.datestr() << "_" << _time.timestr(3)
679 << " OFFBDS " << setw(10) << setprecision(3) << par->xx
680 << " +- " << setw(6) << setprecision(3)
681 << sqrt(_QQ(par->index,par->index));
682 }
683 }
684
685 LOG << endl << endl;
686
687 // Compute dilution of precision
688 // -----------------------------
689 cmpDOP(epoData);
690
691 // Final Message (both log file and screen)
692 // ----------------------------------------
693 LOG << epoData->tt.datestr() << "_" << epoData->tt.timestr(3)
694 << " " << OPT->_roverName
695 << " X = "
696 << setprecision(4) << x() << " +- "
697 << setprecision(4) << sqrt(_QQ(1,1))
698
699 << " Y = "
700 << setprecision(4) << y() << " +- "
701 << setprecision(4) << sqrt(_QQ(2,2))
702
703 << " Z = "
704 << setprecision(4) << z() << " +- "
705 << setprecision(4) << sqrt(_QQ(3,3));
706
707 // NEU Output
708 // ----------
709 if (OPT->xyzAprRoverSet()) {
710 SymmetricMatrix QQxyz = _QQ.SymSubMatrix(1,3);
711
712 ColumnVector xyz(3);
713 xyz(1) = x() - OPT->_xyzAprRover[0];
714 xyz(2) = y() - OPT->_xyzAprRover[1];
715 xyz(3) = z() - OPT->_xyzAprRover[2];
716
717 ColumnVector ellRef(3);
718 xyz2ell(OPT->_xyzAprRover.data(), ellRef.data());
719 xyz2neu(ellRef.data(), xyz.data(), _neu.data());
720
721 SymmetricMatrix QQneu(3);
722 covariXYZ_NEU(QQxyz, ellRef.data(), QQneu);
723
724 LOG << " dN = "
725 << setprecision(4) << _neu[0] << " +- "
726 << setprecision(4) << sqrt(QQneu[0][0])
727
728 << " dE = "
729 << setprecision(4) << _neu[1] << " +- "
730 << setprecision(4) << sqrt(QQneu[1][1])
731
732 << " dU = "
733 << setprecision(4) << _neu[2] << " +- "
734 << setprecision(4) << sqrt(QQneu[2][2]) << endl << endl;
735 }
736 else {
737 LOG << endl << endl;
738 }
739
740 _lastTimeOK = _time; // remember time of last successful update
741 return success;
742}
743
744// Outlier Detection
745////////////////////////////////////////////////////////////////////////////
746QString t_pppFilter::outlierDetection(int iPhase, const ColumnVector& vv,
747 QMap<QString, t_satData*>& satData) {
748
749 Tracer tracer("t_pppFilter::outlierDetection");
750
751 QString prnGPS;
752 QString prnGlo;
753 double maxResGPS = 0.0; // GPS + Galileo
754 double maxResGlo = 0.0; // GLONASS + BDS
755 findMaxRes(vv, satData, prnGPS, prnGlo, maxResGPS, maxResGlo);
756
757 if (iPhase == 1) {
758 if (maxResGlo > 2.98 * OPT->_maxResL1) {
759 LOG << "Outlier Phase " << prnGlo.mid(0,3).toLatin1().data() << ' ' << maxResGlo << endl;
760 return prnGlo;
761 }
762 else if (maxResGPS > MAXRES_PHASE_GPS) {
763 LOG << "Outlier Phase " << prnGPS.mid(0,3).toLatin1().data() << ' ' << maxResGPS << endl;
764 return prnGPS;
765 }
766 }
767 else if (iPhase == 0 && maxResGPS > 2.98 * OPT->_maxResC1) {
768 LOG << "Outlier Code " << prnGPS.mid(0,3).toLatin1().data() << ' ' << maxResGPS << endl;
769 return prnGPS;
770 }
771
772 return QString();
773}
774
775// Phase Wind-Up Correction
776///////////////////////////////////////////////////////////////////////////
777double t_pppFilter::windUp(const QString& prn, const ColumnVector& rSat,
778 const ColumnVector& rRec) {
779
780 Tracer tracer("t_pppFilter::windUp");
781
782 double Mjd = _time.mjd() + _time.daysec() / 86400.0;
783
784 // First time - initialize to zero
785 // -------------------------------
786 if (!_windUpTime.contains(prn)) {
787 _windUpSum[prn] = 0.0;
788 }
789
790 // Compute the correction for new time
791 // -----------------------------------
792 if (!_windUpTime.contains(prn) || _windUpTime[prn] != Mjd) {
793 _windUpTime[prn] = Mjd;
794
795 // Unit Vector GPS Satellite --> Receiver
796 // --------------------------------------
797 ColumnVector rho = rRec - rSat;
798 rho /= rho.norm_Frobenius();
799
800 // GPS Satellite unit Vectors sz, sy, sx
801 // -------------------------------------
802 ColumnVector sz = -rSat / rSat.norm_Frobenius();
803
804 ColumnVector xSun = t_astro::Sun(Mjd);
805 xSun /= xSun.norm_Frobenius();
806
807 ColumnVector sy = crossproduct(sz, xSun);
808 ColumnVector sx = crossproduct(sy, sz);
809
810 // Effective Dipole of the GPS Satellite Antenna
811 // ---------------------------------------------
812 ColumnVector dipSat = sx - rho * DotProduct(rho,sx)
813 - crossproduct(rho, sy);
814
815 // Receiver unit Vectors rx, ry
816 // ----------------------------
817 ColumnVector rx(3);
818 ColumnVector ry(3);
819
820 double recEll[3]; xyz2ell(rRec.data(), recEll) ;
821 double neu[3];
822
823 neu[0] = 1.0;
824 neu[1] = 0.0;
825 neu[2] = 0.0;
826 neu2xyz(recEll, neu, rx.data());
827
828 neu[0] = 0.0;
829 neu[1] = -1.0;
830 neu[2] = 0.0;
831 neu2xyz(recEll, neu, ry.data());
832
833 // Effective Dipole of the Receiver Antenna
834 // ----------------------------------------
835 ColumnVector dipRec = rx - rho * DotProduct(rho,rx)
836 + crossproduct(rho, ry);
837
838 // Resulting Effect
839 // ----------------
840 double alpha = DotProduct(dipSat,dipRec) /
841 (dipSat.norm_Frobenius() * dipRec.norm_Frobenius());
842
843 if (alpha > 1.0) alpha = 1.0;
844 if (alpha < -1.0) alpha = -1.0;
845
846 double dphi = acos(alpha) / 2.0 / M_PI; // in cycles
847
848 if ( DotProduct(rho, crossproduct(dipSat, dipRec)) < 0.0 ) {
849 dphi = -dphi;
850 }
851
852 _windUpSum[prn] = floor(_windUpSum[prn] - dphi + 0.5) + dphi;
853 }
854
855 return _windUpSum[prn];
856}
857
858//
859///////////////////////////////////////////////////////////////////////////
860void t_pppFilter::cmpEle(t_satData* satData) {
861 Tracer tracer("t_pppFilter::cmpEle");
862 ColumnVector rr = satData->xx - _xcBanc.Rows(1,3);
863 double rho = rr.norm_Frobenius();
864
865 double neu[3];
866 xyz2neu(_ellBanc.data(), rr.data(), neu);
867
868 satData->eleSat = acos( sqrt(neu[0]*neu[0] + neu[1]*neu[1]) / rho );
869 if (neu[2] < 0) {
870 satData->eleSat *= -1.0;
871 }
872 satData->azSat = atan2(neu[1], neu[0]);
873}
874
875//
876///////////////////////////////////////////////////////////////////////////
877void t_pppFilter::addAmb(t_satData* satData) {
878 Tracer tracer("t_pppFilter::addAmb");
879 if (!OPT->ambLCs(satData->system()).size()){
880 return;
881 }
882 bool found = false;
883 for (int iPar = 1; iPar <= _params.size(); iPar++) {
884 if (_params[iPar-1]->type == t_pppParam::AMB_L3 &&
885 _params[iPar-1]->prn == satData->prn) {
886 found = true;
887 break;
888 }
889 }
890 if (!found) {
891 t_pppParam* par = new t_pppParam(t_pppParam::AMB_L3,
892 _params.size()+1, satData->prn);
893 _params.push_back(par);
894 par->xx = satData->L3 - cmpValue(satData, true);
895 }
896}
897
898//
899///////////////////////////////////////////////////////////////////////////
900void t_pppFilter::addObs(int iPhase, unsigned& iObs, t_satData* satData,
901 Matrix& AA, ColumnVector& ll, DiagonalMatrix& PP) {
902
903 Tracer tracer("t_pppFilter::addObs");
904
905 const double ELEWGHT = 20.0;
906 double ellWgtCoef = 1.0;
907 double eleD = satData->eleSat * 180.0 / M_PI;
908 if (eleD < ELEWGHT) {
909 ellWgtCoef = 1.5 - 0.5 / (ELEWGHT - 10.0) * (eleD - 10.0);
910 }
911
912 // Remember Observation Index
913 // --------------------------
914 ++iObs;
915 satData->obsIndex = iObs;
916
917 // Phase Observations
918 // ------------------
919
920 if (iPhase == 1) {
921 ll(iObs) = satData->L3 - cmpValue(satData, true);
922 double sigL3 = 2.98 * OPT->_sigmaL1;
923 if (satData->system() == 'R') {
924 sigL3 *= GLONASS_WEIGHT_FACTOR;
925 }
926 if (satData->system() == 'C') {
927 sigL3 *= BDS_WEIGHT_FACTOR;
928 }
929 PP(iObs,iObs) = 1.0 / (sigL3 * sigL3) / (ellWgtCoef * ellWgtCoef);
930 for (int iPar = 1; iPar <= _params.size(); iPar++) {
931 if (_params[iPar-1]->type == t_pppParam::AMB_L3 &&
932 _params[iPar-1]->prn == satData->prn) {
933 ll(iObs) -= _params[iPar-1]->xx;
934 }
935 AA(iObs, iPar) = _params[iPar-1]->partial(satData, true);
936 }
937 }
938
939 // Code Observations
940 // -----------------
941 else {
942 double sigP3 = 2.98 * OPT->_sigmaC1;
943 ll(iObs) = satData->P3 - cmpValue(satData, false);
944 PP(iObs,iObs) = 1.0 / (sigP3 * sigP3) / (ellWgtCoef * ellWgtCoef);
945 for (int iPar = 1; iPar <= _params.size(); iPar++) {
946 AA(iObs, iPar) = _params[iPar-1]->partial(satData, false);
947 }
948 }
949}
950
951//
952///////////////////////////////////////////////////////////////////////////
953QByteArray t_pppFilter::printRes(int iPhase, const ColumnVector& vv,
954 const QMap<QString, t_satData*>& satDataMap) {
955
956 Tracer tracer("t_pppFilter::printRes");
957
958 ostringstream str;
959 str.setf(ios::fixed);
960 bool useObs;
961 QMapIterator<QString, t_satData*> it(satDataMap);
962 while (it.hasNext()) {
963 it.next();
964 t_satData* satData = it.value();
965 (iPhase == 0) ? useObs = OPT->codeLCs(satData->system()).size() :
966 useObs = OPT->ambLCs(satData->system()).size();
967 if (satData->obsIndex != 0 && useObs) {
968 str << _time.datestr() << "_" << _time.timestr(3)
969 << " RES " << satData->prn.mid(0,3).toLatin1().data()
970 << (iPhase ? " L3 " : " P3 ")
971 << setw(9) << setprecision(4) << vv(satData->obsIndex) << endl;
972 }
973 }
974
975 return QByteArray(str.str().c_str());
976}
977
978//
979///////////////////////////////////////////////////////////////////////////
980void t_pppFilter::findMaxRes(const ColumnVector& vv,
981 const QMap<QString, t_satData*>& satData,
982 QString& prnGPS, QString& prnGlo,
983 double& maxResGPS, double& maxResGlo) {
984
985 Tracer tracer("t_pppFilter::findMaxRes");
986
987 maxResGPS = 0.0;
988 maxResGlo = 0.0;
989
990 QMapIterator<QString, t_satData*> it(satData);
991 while (it.hasNext()) {
992 it.next();
993 t_satData* satData = it.value();
994 if (satData->obsIndex != 0) {
995 QString prn = satData->prn;
996 if (prn[0] == 'R' || prn[0] == 'C') {
997 if (fabs(vv(satData->obsIndex)) > maxResGlo) {
998 maxResGlo = fabs(vv(satData->obsIndex));
999 prnGlo = prn;
1000 }
1001 }
1002 else {
1003 if (fabs(vv(satData->obsIndex)) > maxResGPS) {
1004 maxResGPS = fabs(vv(satData->obsIndex));
1005 prnGPS = prn;
1006 }
1007 }
1008 }
1009 }
1010}
1011
1012// Update Step (private - loop over outliers)
1013////////////////////////////////////////////////////////////////////////////
1014t_irc t_pppFilter::update_p(t_epoData* epoData) {
1015
1016 Tracer tracer("t_pppFilter::update_p");
1017
1018 // Save Variance-Covariance Matrix, and Status Vector
1019 // --------------------------------------------------
1020 rememberState(epoData);
1021
1022 QString lastOutlierPrn;
1023
1024 // Try with all satellites, then with all minus one, etc.
1025 // ------------------------------------------------------
1026 while (selectSatellites(lastOutlierPrn, epoData->satData) == success) {
1027
1028 QByteArray strResCode;
1029 QByteArray strResPhase;
1030
1031 // Bancroft Solution
1032 // -----------------
1033 if (cmpBancroft(epoData) != success) {
1034 break;
1035 }
1036
1037 // First update using code observations, then phase observations
1038 // -------------------------------------------------------------
1039 bool usePhase = OPT->ambLCs('G').size() || OPT->ambLCs('R').size() ||
1040 OPT->ambLCs('E').size() || OPT->ambLCs('C').size() ;
1041
1042 char sys[] ={'G', 'R', 'E', 'C'};
1043
1044 bool satnumPrinted[] = {false, false, false, false};
1045
1046 for (int iPhase = 0; iPhase <= (usePhase ? 1 : 0); iPhase++) {
1047
1048 // Status Prediction
1049 // -----------------
1050 predict(iPhase, epoData);
1051
1052 // Create First-Design Matrix
1053 // --------------------------
1054 unsigned nPar = _params.size();
1055 unsigned nObs = 0;
1056 nObs = epoData->sizeAll();
1057 bool useObs = false;
1058 for (unsigned ii = 0; ii < sizeof(sys); ii++) {
1059 const char s = sys[ii];
1060 (iPhase == 0) ? useObs = OPT->codeLCs(s).size() : useObs = OPT->ambLCs(s).size();
1061 if (!useObs) {
1062 nObs -= epoData->sizeSys(s);
1063 }
1064 else {
1065 if (!satnumPrinted[ii]) {
1066 satnumPrinted[ii] = true;
1067 LOG << _time.datestr() << "_" << _time.timestr(3)
1068 << " SATNUM " << s << ' ' << right << setw(2)
1069 << epoData->sizeSys(s) << endl;
1070 }
1071 }
1072 }
1073
1074 if (int(nObs) < OPT->_minObs) {
1075 restoreState(epoData);
1076 return failure;
1077 }
1078
1079 // Prepare first-design Matrix, vector observed-computed
1080 // -----------------------------------------------------
1081 Matrix AA(nObs, nPar); // first design matrix
1082 ColumnVector ll(nObs); // terms observed-computed
1083 DiagonalMatrix PP(nObs); PP = 0.0;
1084
1085 unsigned iObs = 0;
1086 QMapIterator<QString, t_satData*> it(epoData->satData);
1087
1088 while (it.hasNext()) {
1089 it.next();
1090 t_satData* satData = it.value();
1091 QString prn = satData->prn;
1092 (iPhase == 0) ? useObs = OPT->codeLCs(satData->system()).size() :
1093 useObs = OPT->ambLCs(satData->system()).size();
1094 if (useObs) {
1095 addObs(iPhase, iObs, satData, AA, ll, PP);
1096 } else {
1097 satData->obsIndex = 0;
1098 }
1099 }
1100
1101 // Compute Filter Update
1102 // ---------------------
1103 ColumnVector dx(nPar); dx = 0.0;
1104 kalman(AA, ll, PP, _QQ, dx);
1105 ColumnVector vv = ll - AA * dx;
1106
1107 // Print Residuals
1108 // ---------------
1109 if (iPhase == 0) {
1110 strResCode = printRes(iPhase, vv, epoData->satData);
1111 }
1112 else {
1113 strResPhase = printRes(iPhase, vv, epoData->satData);
1114 }
1115
1116 // Check the residuals
1117 // -------------------
1118 lastOutlierPrn = outlierDetection(iPhase, vv, epoData->satData);
1119
1120 // No Outlier Detected
1121 // -------------------
1122 if (lastOutlierPrn.isEmpty()) {
1123
1124 QVectorIterator<t_pppParam*> itPar(_params);
1125 while (itPar.hasNext()) {
1126 t_pppParam* par = itPar.next();
1127 par->xx += dx(par->index);
1128 }
1129
1130 if (!usePhase || iPhase == 1) {
1131 if (_outlierGPS.size() > 0 || _outlierGlo.size() > 0) {
1132 LOG << "Neglected PRNs: ";
1133 if (!_outlierGPS.isEmpty()) {
1134 LOG << _outlierGPS.last().mid(0,3).toLatin1().data() << ' ';
1135 }
1136 QStringListIterator itGlo(_outlierGlo);
1137 while (itGlo.hasNext()) {
1138 QString prn = itGlo.next();
1139 LOG << prn.mid(0,3).toLatin1().data() << ' ';
1140 }
1141 LOG << endl;
1142 }
1143 LOG << strResCode.data() << strResPhase.data();
1144
1145 return success;
1146 }
1147 }
1148
1149 // Outlier Found
1150 // -------------
1151 else {
1152 restoreState(epoData);
1153 break;
1154 }
1155
1156 } // for iPhase
1157
1158 } // while selectSatellites
1159
1160 restoreState(epoData);
1161 return failure;
1162}
1163
1164// Remeber Original State Vector and Variance-Covariance Matrix
1165////////////////////////////////////////////////////////////////////////////
1166void t_pppFilter::rememberState(t_epoData* epoData) {
1167
1168 _QQ_sav = _QQ;
1169
1170 QVectorIterator<t_pppParam*> itSav(_params_sav);
1171 while (itSav.hasNext()) {
1172 t_pppParam* par = itSav.next();
1173 delete par;
1174 }
1175 _params_sav.clear();
1176
1177 QVectorIterator<t_pppParam*> it(_params);
1178 while (it.hasNext()) {
1179 t_pppParam* par = it.next();
1180 _params_sav.push_back(new t_pppParam(*par));
1181 }
1182
1183 _epoData_sav->deepCopy(epoData);
1184}
1185
1186// Restore Original State Vector and Variance-Covariance Matrix
1187////////////////////////////////////////////////////////////////////////////
1188void t_pppFilter::restoreState(t_epoData* epoData) {
1189
1190 _QQ = _QQ_sav;
1191
1192 QVectorIterator<t_pppParam*> it(_params);
1193 while (it.hasNext()) {
1194 t_pppParam* par = it.next();
1195 delete par;
1196 }
1197 _params.clear();
1198
1199 QVectorIterator<t_pppParam*> itSav(_params_sav);
1200 while (itSav.hasNext()) {
1201 t_pppParam* par = itSav.next();
1202 _params.push_back(new t_pppParam(*par));
1203 }
1204
1205 epoData->deepCopy(_epoData_sav);
1206}
1207
1208//
1209////////////////////////////////////////////////////////////////////////////
1210t_irc t_pppFilter::selectSatellites(const QString& lastOutlierPrn,
1211 QMap<QString, t_satData*>& satData) {
1212
1213 // First Call
1214 // ----------
1215 if (lastOutlierPrn.isEmpty()) {
1216 _outlierGPS.clear();
1217 _outlierGlo.clear();
1218 return success;
1219 }
1220
1221 // Second and next trials
1222 // ----------------------
1223 else {
1224
1225 if (lastOutlierPrn[0] == 'R' || lastOutlierPrn[0] == 'C') {
1226 _outlierGlo << lastOutlierPrn;
1227 }
1228
1229 // Remove all Glonass Outliers
1230 // ---------------------------
1231 QStringListIterator it(_outlierGlo);
1232 while (it.hasNext()) {
1233 QString prn = it.next();
1234 if (satData.contains(prn)) {
1235 delete satData.take(prn);
1236 }
1237 }
1238
1239 if (lastOutlierPrn[0] == 'R' || lastOutlierPrn[0] == 'C') {
1240 _outlierGPS.clear();
1241 return success;
1242 }
1243
1244 // GPS Outlier appeared for the first time - try to delete it
1245 // ----------------------------------------------------------
1246 if (_outlierGPS.indexOf(lastOutlierPrn) == -1) {
1247 _outlierGPS << lastOutlierPrn;
1248 if (satData.contains(lastOutlierPrn)) {
1249 delete satData.take(lastOutlierPrn);
1250 }
1251 return success;
1252 }
1253
1254 }
1255
1256 return failure;
1257}
1258
1259//
1260////////////////////////////////////////////////////////////////////////////
1261double lorentz(const ColumnVector& aa, const ColumnVector& bb) {
1262 return aa(1)*bb(1) + aa(2)*bb(2) + aa(3)*bb(3) - aa(4)*bb(4);
1263}
1264
1265//
1266////////////////////////////////////////////////////////////////////////////
1267void t_pppFilter::bancroft(const Matrix& BBpass, ColumnVector& pos) {
1268
1269 if (pos.Nrows() != 4) {
1270 pos.ReSize(4);
1271 }
1272 pos = 0.0;
1273
1274 for (int iter = 1; iter <= 2; iter++) {
1275 Matrix BB = BBpass;
1276 int mm = BB.Nrows();
1277 for (int ii = 1; ii <= mm; ii++) {
1278 double xx = BB(ii,1);
1279 double yy = BB(ii,2);
1280 double traveltime = 0.072;
1281 if (iter > 1) {
1282 double zz = BB(ii,3);
1283 double rho = sqrt( (xx-pos(1)) * (xx-pos(1)) +
1284 (yy-pos(2)) * (yy-pos(2)) +
1285 (zz-pos(3)) * (zz-pos(3)) );
1286 traveltime = rho / t_CST::c;
1287 }
1288 double angle = traveltime * t_CST::omega;
1289 double cosa = cos(angle);
1290 double sina = sin(angle);
1291 BB(ii,1) = cosa * xx + sina * yy;
1292 BB(ii,2) = -sina * xx + cosa * yy;
1293 }
1294
1295 Matrix BBB;
1296 if (mm > 4) {
1297 SymmetricMatrix hlp; hlp << BB.t() * BB;
1298 BBB = hlp.i() * BB.t();
1299 }
1300 else {
1301 BBB = BB.i();
1302 }
1303 ColumnVector ee(mm); ee = 1.0;
1304 ColumnVector alpha(mm); alpha = 0.0;
1305 for (int ii = 1; ii <= mm; ii++) {
1306 alpha(ii) = lorentz(BB.Row(ii).t(),BB.Row(ii).t())/2.0;
1307 }
1308 ColumnVector BBBe = BBB * ee;
1309 ColumnVector BBBalpha = BBB * alpha;
1310 double aa = lorentz(BBBe, BBBe);
1311 double bb = lorentz(BBBe, BBBalpha)-1;
1312 double cc = lorentz(BBBalpha, BBBalpha);
1313 double root = sqrt(bb*bb-aa*cc);
1314
1315 Matrix hlpPos(4,2);
1316 hlpPos.Column(1) = (-bb-root)/aa * BBBe + BBBalpha;
1317 hlpPos.Column(2) = (-bb+root)/aa * BBBe + BBBalpha;
1318
1319 ColumnVector omc(2);
1320 for (int pp = 1; pp <= 2; pp++) {
1321 hlpPos(4,pp) = -hlpPos(4,pp);
1322 omc(pp) = BB(1,4) -
1323 sqrt( (BB(1,1)-hlpPos(1,pp)) * (BB(1,1)-hlpPos(1,pp)) +
1324 (BB(1,2)-hlpPos(2,pp)) * (BB(1,2)-hlpPos(2,pp)) +
1325 (BB(1,3)-hlpPos(3,pp)) * (BB(1,3)-hlpPos(3,pp)) ) -
1326 hlpPos(4,pp);
1327 }
1328 if ( fabs(omc(1)) > fabs(omc(2)) ) {
1329 pos = hlpPos.Column(2);
1330 }
1331 else {
1332 pos = hlpPos.Column(1);
1333 }
1334 }
1335}
1336
1337//
1338////////////////////////////////////////////////////////////////////////////
1339void t_pppFilter::cmpDOP(t_epoData* epoData) {
1340
1341 Tracer tracer("t_pppFilter::cmpDOP");
1342
1343 _numSat = 0;
1344 _hDop = 0.0;
1345
1346 if (_params.size() < 4) {
1347 return;
1348 }
1349
1350 const unsigned numPar = 4;
1351 Matrix AA(epoData->sizeAll(), numPar);
1352 QMapIterator<QString, t_satData*> it(epoData->satData);
1353 while (it.hasNext()) {
1354 it.next();
1355 t_satData* satData = it.value();
1356 _numSat += 1;
1357 for (unsigned iPar = 0; iPar < numPar; iPar++) {
1358 AA[_numSat-1][iPar] = _params[iPar]->partial(satData, false);
1359 }
1360 }
1361 if (_numSat < 4) {
1362 return;
1363 }
1364 AA = AA.Rows(1, _numSat);
1365 SymmetricMatrix NN; NN << AA.t() * AA;
1366 SymmetricMatrix QQ = NN.i();
1367
1368 _hDop = sqrt(QQ(1,1) + QQ(2,2));
1369}
Note: See TracBrowser for help on using the repository browser.