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

Last change on this file since 9083 was 9083, checked in by stuerze, 22 months ago

minor changes

  • 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.6 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 = 5.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).NormFrobenius();
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->earth(_time, xRec);
342
343 satData->rho = (satData->xx - xRec).NormFrobenius();
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
355 t_frequency::type frqA = t_frequency::G1;
356 t_frequency::type frqB = t_frequency::G2;
357 if (satData->prn[0] == 'R') {
358 offset = Glonass_offset();
359 frqA = t_frequency::R1;
360 frqB = t_frequency::R2;
361 }
362 else if (satData->prn[0] == 'E') {
363 offset = Galileo_offset();
364 frqA = t_frequency::E1;
365 frqB = t_frequency::E5;
366 }
367 else if (satData->prn[0] == 'C') {
368 offset = Bds_offset();
369 frqA = t_frequency::C2;
370 frqB = t_frequency::C7;
371 }
372 double phaseCenter = 0.0;
373 if (_antex) {
374 bool found;
375 phaseCenter = satData->lkA * _antex->rcvCorr(OPT->_antNameRover, frqA,
376 satData->eleSat, satData->azSat,
377 found)
378 + satData->lkB * _antex->rcvCorr(OPT->_antNameRover, frqB,
379 satData->eleSat, satData->azSat,
380 found);
381 if (!found) {
382 LOG << "ANTEX: antenna >" << OPT->_antNameRover << "< not found\n";
383 }
384 }
385
386 double antennaOffset = 0.0;
387 double cosa = cos(satData->azSat);
388 double sina = sin(satData->azSat);
389 double cose = cos(satData->eleSat);
390 double sine = sin(satData->eleSat);
391 antennaOffset = -OPT->_neuEccRover(1) * cosa*cose
392 -OPT->_neuEccRover(2) * sina*cose
393 -OPT->_neuEccRover(3) * sine;
394
395 return satData->rho + phaseCenter + antennaOffset + clk()
396 + offset - satData->clk + tropDelay + wind;
397}
398
399// Tropospheric Model (Saastamoinen)
400////////////////////////////////////////////////////////////////////////////
401double t_pppFilter::delay_saast(double Ele) {
402
403 Tracer tracer("t_pppFilter::delay_saast");
404
405 double xyz[3];
406 xyz[0] = x();
407 xyz[1] = y();
408 xyz[2] = z();
409 double ell[3];
410 xyz2ell(xyz, ell);
411 double height = ell[2];
412
413 double pp = 1013.25 * pow(1.0 - 2.26e-5 * height, 5.225);
414 double TT = 18.0 - height * 0.0065 + 273.15;
415 double hh = 50.0 * exp(-6.396e-4 * height);
416 double ee = hh / 100.0 * exp(-37.2465 + 0.213166*TT - 0.000256908*TT*TT);
417
418 double h_km = height / 1000.0;
419
420 if (h_km < 0.0) h_km = 0.0;
421 if (h_km > 5.0) h_km = 5.0;
422 int ii = int(h_km + 1);
423 double href = ii - 1;
424
425 double bCor[6];
426 bCor[0] = 1.156;
427 bCor[1] = 1.006;
428 bCor[2] = 0.874;
429 bCor[3] = 0.757;
430 bCor[4] = 0.654;
431 bCor[5] = 0.563;
432
433 double BB = bCor[ii-1] + (bCor[ii]-bCor[ii-1]) * (h_km - href);
434
435 double zen = M_PI/2.0 - Ele;
436
437 return (0.002277/cos(zen)) * (pp + ((1255.0/TT)+0.05)*ee - BB*(tan(zen)*tan(zen)));
438}
439
440// Prediction Step of the Filter
441////////////////////////////////////////////////////////////////////////////
442void t_pppFilter::predict(int iPhase, t_epoData* epoData) {
443
444 Tracer tracer("t_pppFilter::predict");
445
446 if (iPhase == 0) {
447
448 const double maxSolGap = 60.0;
449
450 bool firstCrd = false;
451 if (!_lastTimeOK.valid() || (maxSolGap > 0.0 && _time - _lastTimeOK > maxSolGap)) {
452 firstCrd = true;
453 _startTime = epoData->tt;
454 reset();
455 }
456
457 // Use different white noise for Quick-Start mode
458 // ----------------------------------------------
459 double sigCrdP_used = OPT->_noiseCrd(1);
460 if ( OPT->_seedingTime > 0.0 && OPT->_seedingTime > (epoData->tt - _startTime) ) {
461 sigCrdP_used = 0.0;
462 }
463
464 // Predict Parameter values, add white noise
465 // -----------------------------------------
466 for (int iPar = 1; iPar <= _params.size(); iPar++) {
467 t_pppParam* pp = _params[iPar-1];
468
469 // Coordinates
470 // -----------
471 if (pp->type == t_pppParam::CRD_X) {
472 if (firstCrd) {
473 if (OPT->xyzAprRoverSet()) {
474 pp->xx = OPT->_xyzAprRover[0];
475 }
476 else {
477 pp->xx = _xcBanc(1);
478 }
479 }
480 _QQ(iPar,iPar) += sigCrdP_used * sigCrdP_used;
481 }
482 else if (pp->type == t_pppParam::CRD_Y) {
483 if (firstCrd) {
484 if (OPT->xyzAprRoverSet()) {
485 pp->xx = OPT->_xyzAprRover[1];
486 }
487 else {
488 pp->xx = _xcBanc(2);
489 }
490 }
491 _QQ(iPar,iPar) += sigCrdP_used * sigCrdP_used;
492 }
493 else if (pp->type == t_pppParam::CRD_Z) {
494 if (firstCrd) {
495 if (OPT->xyzAprRoverSet()) {
496 pp->xx = OPT->_xyzAprRover[2];
497 }
498 else {
499 pp->xx = _xcBanc(3);
500 }
501 }
502 _QQ(iPar,iPar) += sigCrdP_used * sigCrdP_used;
503 }
504
505 // Receiver Clocks
506 // ---------------
507 else if (pp->type == t_pppParam::RECCLK) {
508 pp->xx = _xcBanc(4);
509 for (int jj = 1; jj <= _params.size(); jj++) {
510 _QQ(iPar, jj) = 0.0;
511 }
512 _QQ(iPar,iPar) = OPT->_noiseClk * OPT->_noiseClk;
513 }
514
515 // Tropospheric Delay
516 // ------------------
517 else if (pp->type == t_pppParam::TROPO) {
518 _QQ(iPar,iPar) += OPT->_noiseTrp * OPT->_noiseTrp;
519 }
520
521 // Glonass Offset
522 // --------------
523 else if (pp->type == t_pppParam::GLONASS_OFFSET) {
524 pp->xx = 0.0;
525 for (int jj = 1; jj <= _params.size(); jj++) {
526 _QQ(iPar, jj) = 0.0;
527 }
528 _QQ(iPar,iPar) = 1000.0 * 1000.0;
529 }
530
531 // Galileo Offset
532 // --------------
533 else if (pp->type == t_pppParam::GALILEO_OFFSET) {
534 _QQ(iPar,iPar) += 0.1 * 0.1;
535 }
536
537 // BDS Offset
538 // ----------
539 else if (pp->type == t_pppParam::BDS_OFFSET) {
540 _QQ(iPar,iPar) += 0.1 * 0.1; //TODO: TEST
541 }
542 }
543 }
544
545 // Add New Ambiguities if necessary
546 // --------------------------------
547 if (OPT->ambLCs('G').size() || OPT->ambLCs('R').size() ||
548 OPT->ambLCs('E').size() || OPT->ambLCs('C').size()) {
549
550 // Make a copy of QQ and xx, set parameter indices
551 // -----------------------------------------------
552 SymmetricMatrix QQ_old = _QQ;
553
554 for (int iPar = 1; iPar <= _params.size(); iPar++) {
555 _params[iPar-1]->index_old = _params[iPar-1]->index;
556 _params[iPar-1]->index = 0;
557 }
558
559 // Remove Ambiguity Parameters without observations
560 // ------------------------------------------------
561 int iPar = 0;
562 QMutableVectorIterator<t_pppParam*> im(_params);
563 while (im.hasNext()) {
564 t_pppParam* par = im.next();
565 bool removed = false;
566 if (par->type == t_pppParam::AMB_L3) {
567 if (epoData->satData.find(par->prn) == epoData->satData.end()) {
568 removed = true;
569 delete par;
570 im.remove();
571 }
572 }
573 if (! removed) {
574 ++iPar;
575 par->index = iPar;
576 }
577 }
578
579 // Add new ambiguity parameters
580 // ----------------------------
581 QMapIterator<QString, t_satData*> it(epoData->satData);
582 while (it.hasNext()) {
583 it.next();
584 t_satData* satData = it.value();
585 addAmb(satData);
586 }
587
588 int nPar = _params.size();
589 _QQ.ReSize(nPar); _QQ = 0.0;
590 for (int i1 = 1; i1 <= nPar; i1++) {
591 t_pppParam* p1 = _params[i1-1];
592 if (p1->index_old != 0) {
593 _QQ(p1->index, p1->index) = QQ_old(p1->index_old, p1->index_old);
594 for (int i2 = 1; i2 <= nPar; i2++) {
595 t_pppParam* p2 = _params[i2-1];
596 if (p2->index_old != 0) {
597 _QQ(p1->index, p2->index) = QQ_old(p1->index_old, p2->index_old);
598 }
599 }
600 }
601 }
602
603 for (int ii = 1; ii <= nPar; ii++) {
604 t_pppParam* par = _params[ii-1];
605 if (par->index_old == 0) {
606 _QQ(par->index, par->index) = OPT->_aprSigAmb * OPT->_aprSigAmb;
607 }
608 par->index_old = par->index;
609 }
610 }
611}
612
613// Update Step of the Filter (currently just a single-epoch solution)
614////////////////////////////////////////////////////////////////////////////
615t_irc t_pppFilter::update(t_epoData* epoData) {
616
617 Tracer tracer("t_pppFilter::update");
618
619 _time = epoData->tt; // current epoch time
620
621 if (OPT->useOrbClkCorr()) {
622 LOG << "Precise Point Positioning of Epoch " << _time.datestr() << "_" << _time.timestr(3)
623 << "\n---------------------------------------------------------------\n";
624 }
625 else {
626 LOG << "Single Point Positioning of Epoch " << _time.datestr() << "_" << _time.timestr(3)
627 << "\n---------------------------------------------------------------\n";
628 }
629
630 // Outlier Detection Loop
631 // ----------------------
632 if (update_p(epoData) != success) {
633 return failure;
634 }
635
636 // Set Solution Vector
637 // -------------------
638 LOG.setf(ios::fixed);
639 QVectorIterator<t_pppParam*> itPar(_params);
640 while (itPar.hasNext()) {
641 t_pppParam* par = itPar.next();
642 if (par->type == t_pppParam::RECCLK) {
643 LOG << "\n" << _time.datestr() << "_" << _time.timestr(3)
644 << " CLK " << setw(10) << setprecision(3) << par->xx
645 << " +- " << setw(6) << setprecision(3)
646 << sqrt(_QQ(par->index,par->index));
647 }
648 else if (par->type == t_pppParam::AMB_L3) {
649 ++par->numEpo;
650 LOG << "\n" << _time.datestr() << "_" << _time.timestr(3)
651 << " AMB " << par->prn.mid(0,3).toLatin1().data() << " "
652 << setw(10) << setprecision(3) << par->xx
653 << " +- " << setw(6) << setprecision(3)
654 << sqrt(_QQ(par->index,par->index))
655 << " epo = " << par->numEpo;
656 }
657 else if (par->type == t_pppParam::TROPO) {
658 double aprTrp = delay_saast(M_PI/2.0);
659 LOG << "\n" << _time.datestr() << "_" << _time.timestr(3)
660 << " TRP " << par->prn.mid(0,3).toLatin1().data()
661 << setw(7) << setprecision(3) << aprTrp << " "
662 << setw(6) << setprecision(3) << showpos << par->xx << noshowpos
663 << " +- " << setw(6) << setprecision(3)
664 << sqrt(_QQ(par->index,par->index));
665 }
666 else if (par->type == t_pppParam::GLONASS_OFFSET) {
667 LOG << "\n" << _time.datestr() << "_" << _time.timestr(3)
668 << " OFFGLO " << setw(10) << setprecision(3) << par->xx
669 << " +- " << setw(6) << setprecision(3)
670 << sqrt(_QQ(par->index,par->index));
671 }
672 else if (par->type == t_pppParam::GALILEO_OFFSET) {
673 LOG << "\n" << _time.datestr() << "_" << _time.timestr(3)
674 << " OFFGAL " << setw(10) << setprecision(3) << par->xx
675 << " +- " << setw(6) << setprecision(3)
676 << sqrt(_QQ(par->index,par->index));
677 }
678 else if (par->type == t_pppParam::BDS_OFFSET) {
679 LOG << "\n" << _time.datestr() << "_" << _time.timestr(3)
680 << " OFFBDS " << setw(10) << setprecision(3) << par->xx
681 << " +- " << setw(6) << setprecision(3)
682 << sqrt(_QQ(par->index,par->index));
683 }
684 }
685
686 LOG << endl << endl;
687
688 // Compute dilution of precision
689 // -----------------------------
690 cmpDOP(epoData);
691
692 // Final Message (both log file and screen)
693 // ----------------------------------------
694 LOG << epoData->tt.datestr() << "_" << epoData->tt.timestr(3)
695 << " " << OPT->_roverName
696 << " X = "
697 << setprecision(4) << x() << " +- "
698 << setprecision(4) << sqrt(_QQ(1,1))
699
700 << " Y = "
701 << setprecision(4) << y() << " +- "
702 << setprecision(4) << sqrt(_QQ(2,2))
703
704 << " Z = "
705 << setprecision(4) << z() << " +- "
706 << setprecision(4) << sqrt(_QQ(3,3));
707
708 // NEU Output
709 // ----------
710 if (OPT->xyzAprRoverSet()) {
711 SymmetricMatrix QQxyz = _QQ.SymSubMatrix(1,3);
712
713 ColumnVector xyz(3);
714 xyz(1) = x() - OPT->_xyzAprRover[0];
715 xyz(2) = y() - OPT->_xyzAprRover[1];
716 xyz(3) = z() - OPT->_xyzAprRover[2];
717
718 ColumnVector ellRef(3);
719 xyz2ell(OPT->_xyzAprRover.data(), ellRef.data());
720 xyz2neu(ellRef.data(), xyz.data(), _neu.data());
721
722 SymmetricMatrix QQneu(3);
723 covariXYZ_NEU(QQxyz, ellRef.data(), QQneu);
724
725 LOG << " dN = "
726 << setprecision(4) << _neu[0] << " +- "
727 << setprecision(4) << sqrt(QQneu[0][0])
728
729 << " dE = "
730 << setprecision(4) << _neu[1] << " +- "
731 << setprecision(4) << sqrt(QQneu[1][1])
732
733 << " dU = "
734 << setprecision(4) << _neu[2] << " +- "
735 << setprecision(4) << sqrt(QQneu[2][2]) << endl << endl;
736 }
737 else {
738 LOG << endl << endl;
739 }
740
741 _lastTimeOK = _time; // remember time of last successful update
742 return success;
743}
744
745// Outlier Detection
746////////////////////////////////////////////////////////////////////////////
747QString t_pppFilter::outlierDetection(int iPhase, const ColumnVector& vv,
748 QMap<QString, t_satData*>& satData) {
749
750 Tracer tracer("t_pppFilter::outlierDetection");
751
752 QString prnGPS;
753 QString prnGlo;
754 double maxResGPS = 0.0; // GPS + Galileo
755 double maxResGlo = 0.0; // GLONASS + BDS
756 findMaxRes(vv, satData, prnGPS, prnGlo, maxResGPS, maxResGlo);
757
758 if (iPhase == 1) {
759 if (maxResGlo > 2.98 * OPT->_maxResL1) {
760 LOG << "Outlier Phase " << prnGlo.mid(0,3).toLatin1().data() << ' ' << maxResGlo << endl;
761 return prnGlo;
762 }
763 else if (maxResGPS > MAXRES_PHASE_GPS) {
764 LOG << "Outlier Phase " << prnGPS.mid(0,3).toLatin1().data() << ' ' << maxResGPS << endl;
765 return prnGPS;
766 }
767 }
768 else if (iPhase == 0 && maxResGPS > 2.98 * OPT->_maxResC1) {
769 LOG << "Outlier Code " << prnGPS.mid(0,3).toLatin1().data() << ' ' << maxResGPS << endl;
770 return prnGPS;
771 }
772
773 return QString();
774}
775
776// Phase Wind-Up Correction
777///////////////////////////////////////////////////////////////////////////
778double t_pppFilter::windUp(const QString& prn, const ColumnVector& rSat,
779 const ColumnVector& rRec) {
780
781 Tracer tracer("t_pppFilter::windUp");
782
783 double Mjd = _time.mjd() + _time.daysec() / 86400.0;
784
785 // First time - initialize to zero
786 // -------------------------------
787 if (!_windUpTime.contains(prn)) {
788 _windUpSum[prn] = 0.0;
789 }
790
791 // Compute the correction for new time
792 // -----------------------------------
793 if (!_windUpTime.contains(prn) || _windUpTime[prn] != Mjd) {
794 _windUpTime[prn] = Mjd;
795
796 // Unit Vector GPS Satellite --> Receiver
797 // --------------------------------------
798 ColumnVector rho = rRec - rSat;
799 rho /= rho.NormFrobenius();
800
801 // GPS Satellite unit Vectors sz, sy, sx
802 // -------------------------------------
803 ColumnVector sz = -rSat / rSat.NormFrobenius();
804
805 ColumnVector xSun = t_astro::Sun(Mjd);
806 xSun /= xSun.NormFrobenius();
807
808 ColumnVector sy = crossproduct(sz, xSun);
809 ColumnVector sx = crossproduct(sy, sz);
810
811 // Effective Dipole of the GPS Satellite Antenna
812 // ---------------------------------------------
813 ColumnVector dipSat = sx - rho * DotProduct(rho,sx)
814 - crossproduct(rho, sy);
815
816 // Receiver unit Vectors rx, ry
817 // ----------------------------
818 ColumnVector rx(3);
819 ColumnVector ry(3);
820
821 double recEll[3]; xyz2ell(rRec.data(), recEll) ;
822 double neu[3];
823
824 neu[0] = 1.0;
825 neu[1] = 0.0;
826 neu[2] = 0.0;
827 neu2xyz(recEll, neu, rx.data());
828
829 neu[0] = 0.0;
830 neu[1] = -1.0;
831 neu[2] = 0.0;
832 neu2xyz(recEll, neu, ry.data());
833
834 // Effective Dipole of the Receiver Antenna
835 // ----------------------------------------
836 ColumnVector dipRec = rx - rho * DotProduct(rho,rx)
837 + crossproduct(rho, ry);
838
839 // Resulting Effect
840 // ----------------
841 double alpha = DotProduct(dipSat,dipRec) /
842 (dipSat.NormFrobenius() * dipRec.NormFrobenius());
843
844 if (alpha > 1.0) alpha = 1.0;
845 if (alpha < -1.0) alpha = -1.0;
846
847 double dphi = acos(alpha) / 2.0 / M_PI; // in cycles
848
849 if ( DotProduct(rho, crossproduct(dipSat, dipRec)) < 0.0 ) {
850 dphi = -dphi;
851 }
852
853 _windUpSum[prn] = floor(_windUpSum[prn] - dphi + 0.5) + dphi;
854 }
855
856 return _windUpSum[prn];
857}
858
859//
860///////////////////////////////////////////////////////////////////////////
861void t_pppFilter::cmpEle(t_satData* satData) {
862 Tracer tracer("t_pppFilter::cmpEle");
863 ColumnVector rr = satData->xx - _xcBanc.Rows(1,3);
864 double rho = rr.NormFrobenius();
865
866 double neu[3];
867 xyz2neu(_ellBanc.data(), rr.data(), neu);
868
869 satData->eleSat = acos( sqrt(neu[0]*neu[0] + neu[1]*neu[1]) / rho );
870 if (neu[2] < 0) {
871 satData->eleSat *= -1.0;
872 }
873 satData->azSat = atan2(neu[1], neu[0]);
874}
875
876//
877///////////////////////////////////////////////////////////////////////////
878void t_pppFilter::addAmb(t_satData* satData) {
879 Tracer tracer("t_pppFilter::addAmb");
880 if (!OPT->ambLCs(satData->system()).size()){
881 return;
882 }
883 bool found = false;
884 for (int iPar = 1; iPar <= _params.size(); iPar++) {
885 if (_params[iPar-1]->type == t_pppParam::AMB_L3 &&
886 _params[iPar-1]->prn == satData->prn) {
887 found = true;
888 break;
889 }
890 }
891 if (!found) {
892 t_pppParam* par = new t_pppParam(t_pppParam::AMB_L3,
893 _params.size()+1, satData->prn);
894 _params.push_back(par);
895 par->xx = satData->L3 - cmpValue(satData, true);
896 }
897}
898
899//
900///////////////////////////////////////////////////////////////////////////
901void t_pppFilter::addObs(int iPhase, unsigned& iObs, t_satData* satData,
902 Matrix& AA, ColumnVector& ll, DiagonalMatrix& PP) {
903
904 Tracer tracer("t_pppFilter::addObs");
905
906 const double ELEWGHT = 20.0;
907 double ellWgtCoef = 1.0;
908 double eleD = satData->eleSat * 180.0 / M_PI;
909 if (eleD < ELEWGHT) {
910 ellWgtCoef = 1.5 - 0.5 / (ELEWGHT - 10.0) * (eleD - 10.0);
911 }
912
913 // Remember Observation Index
914 // --------------------------
915 ++iObs;
916 satData->obsIndex = iObs;
917
918 // Phase Observations
919 // ------------------
920
921 if (iPhase == 1) {
922 ll(iObs) = satData->L3 - cmpValue(satData, true);
923 double sigL3 = 2.98 * OPT->_sigmaL1;
924 if (satData->system() == 'R') {
925 sigL3 *= GLONASS_WEIGHT_FACTOR;
926 }
927 if (satData->system() == 'C') {
928 sigL3 *= BDS_WEIGHT_FACTOR;
929 }
930 PP(iObs,iObs) = 1.0 / (sigL3 * sigL3) / (ellWgtCoef * ellWgtCoef);
931 for (int iPar = 1; iPar <= _params.size(); iPar++) {
932 if (_params[iPar-1]->type == t_pppParam::AMB_L3 &&
933 _params[iPar-1]->prn == satData->prn) {
934 ll(iObs) -= _params[iPar-1]->xx;
935 }
936 AA(iObs, iPar) = _params[iPar-1]->partial(satData, true);
937 }
938 }
939
940 // Code Observations
941 // -----------------
942 else {
943 double sigP3 = 2.98 * OPT->_sigmaC1;
944 ll(iObs) = satData->P3 - cmpValue(satData, false);
945 PP(iObs,iObs) = 1.0 / (sigP3 * sigP3) / (ellWgtCoef * ellWgtCoef);
946 for (int iPar = 1; iPar <= _params.size(); iPar++) {
947 AA(iObs, iPar) = _params[iPar-1]->partial(satData, false);
948 }
949 }
950}
951
952//
953///////////////////////////////////////////////////////////////////////////
954QByteArray t_pppFilter::printRes(int iPhase, const ColumnVector& vv,
955 const QMap<QString, t_satData*>& satDataMap) {
956
957 Tracer tracer("t_pppFilter::printRes");
958
959 ostringstream str;
960 str.setf(ios::fixed);
961 bool useObs;
962 QMapIterator<QString, t_satData*> it(satDataMap);
963 while (it.hasNext()) {
964 it.next();
965 t_satData* satData = it.value();
966 (iPhase == 0) ? useObs = OPT->codeLCs(satData->system()).size() :
967 useObs = OPT->ambLCs(satData->system()).size();
968 if (satData->obsIndex != 0 && useObs) {
969 str << _time.datestr() << "_" << _time.timestr(3)
970 << " RES " << satData->prn.mid(0,3).toLatin1().data()
971 << (iPhase ? " L3 " : " P3 ")
972 << setw(9) << setprecision(4) << vv(satData->obsIndex) << endl;
973 }
974 }
975
976 return QByteArray(str.str().c_str());
977}
978
979//
980///////////////////////////////////////////////////////////////////////////
981void t_pppFilter::findMaxRes(const ColumnVector& vv,
982 const QMap<QString, t_satData*>& satData,
983 QString& prnGPS, QString& prnGlo,
984 double& maxResGPS, double& maxResGlo) {
985
986 Tracer tracer("t_pppFilter::findMaxRes");
987
988 maxResGPS = 0.0;
989 maxResGlo = 0.0;
990
991 QMapIterator<QString, t_satData*> it(satData);
992 while (it.hasNext()) {
993 it.next();
994 t_satData* satData = it.value();
995 if (satData->obsIndex != 0) {
996 QString prn = satData->prn;
997 if (prn[0] == 'R' || prn[0] == 'C') {
998 if (fabs(vv(satData->obsIndex)) > maxResGlo) {
999 maxResGlo = fabs(vv(satData->obsIndex));
1000 prnGlo = prn;
1001 }
1002 }
1003 else {
1004 if (fabs(vv(satData->obsIndex)) > maxResGPS) {
1005 maxResGPS = fabs(vv(satData->obsIndex));
1006 prnGPS = prn;
1007 }
1008 }
1009 }
1010 }
1011}
1012
1013// Update Step (private - loop over outliers)
1014////////////////////////////////////////////////////////////////////////////
1015t_irc t_pppFilter::update_p(t_epoData* epoData) {
1016
1017 Tracer tracer("t_pppFilter::update_p");
1018
1019 // Save Variance-Covariance Matrix, and Status Vector
1020 // --------------------------------------------------
1021 rememberState(epoData);
1022
1023 QString lastOutlierPrn;
1024
1025 // Try with all satellites, then with all minus one, etc.
1026 // ------------------------------------------------------
1027 while (selectSatellites(lastOutlierPrn, epoData->satData) == success) {
1028
1029 QByteArray strResCode;
1030 QByteArray strResPhase;
1031
1032 // Bancroft Solution
1033 // -----------------
1034 if (cmpBancroft(epoData) != success) {
1035 break;
1036 }
1037
1038 // First update using code observations, then phase observations
1039 // -------------------------------------------------------------
1040 bool usePhase = OPT->ambLCs('G').size() || OPT->ambLCs('R').size() ||
1041 OPT->ambLCs('E').size() || OPT->ambLCs('C').size() ;
1042
1043 char sys[] ={'G', 'R', 'E', 'C'};
1044
1045 bool satnumPrinted[] = {false, false, false, false};
1046
1047 for (int iPhase = 0; iPhase <= (usePhase ? 1 : 0); iPhase++) {
1048
1049 // Status Prediction
1050 // -----------------
1051 predict(iPhase, epoData);
1052
1053 // Create First-Design Matrix
1054 // --------------------------
1055 unsigned nPar = _params.size();
1056 unsigned nObs = 0;
1057 nObs = epoData->sizeAll();
1058 bool useObs = false;
1059 for (unsigned ii = 0; ii < sizeof(sys); ii++) {
1060 const char s = sys[ii];
1061 (iPhase == 0) ? useObs = OPT->codeLCs(s).size() : useObs = OPT->ambLCs(s).size();
1062 if (!useObs) {
1063 nObs -= epoData->sizeSys(s);
1064 }
1065 else {
1066 if (!satnumPrinted[ii]) {
1067 satnumPrinted[ii] = true;
1068 LOG << _time.datestr() << "_" << _time.timestr(3)
1069 << " SATNUM " << s << ' ' << right << setw(2)
1070 << epoData->sizeSys(s) << endl;
1071 }
1072 }
1073 }
1074
1075 if (int(nObs) < OPT->_minObs) {
1076 restoreState(epoData);
1077 return failure;
1078 }
1079
1080 // Prepare first-design Matrix, vector observed-computed
1081 // -----------------------------------------------------
1082 Matrix AA(nObs, nPar); // first design matrix
1083 ColumnVector ll(nObs); // terms observed-computed
1084 DiagonalMatrix PP(nObs); PP = 0.0;
1085
1086 unsigned iObs = 0;
1087 QMapIterator<QString, t_satData*> it(epoData->satData);
1088
1089 while (it.hasNext()) {
1090 it.next();
1091 t_satData* satData = it.value();
1092 QString prn = satData->prn;
1093 (iPhase == 0) ? useObs = OPT->codeLCs(satData->system()).size() :
1094 useObs = OPT->ambLCs(satData->system()).size();
1095 if (useObs) {
1096 addObs(iPhase, iObs, satData, AA, ll, PP);
1097 } else {
1098 satData->obsIndex = 0;
1099 }
1100 }
1101
1102 // Compute Filter Update
1103 // ---------------------
1104 ColumnVector dx(nPar); dx = 0.0;
1105 kalman(AA, ll, PP, _QQ, dx);
1106 ColumnVector vv = ll - AA * dx;
1107
1108 // Print Residuals
1109 // ---------------
1110 if (iPhase == 0) {
1111 strResCode = printRes(iPhase, vv, epoData->satData);
1112 }
1113 else {
1114 strResPhase = printRes(iPhase, vv, epoData->satData);
1115 }
1116
1117 // Check the residuals
1118 // -------------------
1119 lastOutlierPrn = outlierDetection(iPhase, vv, epoData->satData);
1120
1121 // No Outlier Detected
1122 // -------------------
1123 if (lastOutlierPrn.isEmpty()) {
1124
1125 QVectorIterator<t_pppParam*> itPar(_params);
1126 while (itPar.hasNext()) {
1127 t_pppParam* par = itPar.next();
1128 par->xx += dx(par->index);
1129 }
1130
1131 if (!usePhase || iPhase == 1) {
1132 if (_outlierGPS.size() > 0 || _outlierGlo.size() > 0) {
1133 LOG << "Neglected PRNs: ";
1134 if (!_outlierGPS.isEmpty()) {
1135 LOG << _outlierGPS.last().mid(0,3).toLatin1().data() << ' ';
1136 }
1137 QStringListIterator itGlo(_outlierGlo);
1138 while (itGlo.hasNext()) {
1139 QString prn = itGlo.next();
1140 LOG << prn.mid(0,3).toLatin1().data() << ' ';
1141 }
1142 LOG << endl;
1143 }
1144 LOG << strResCode.data() << strResPhase.data();
1145
1146 return success;
1147 }
1148 }
1149
1150 // Outlier Found
1151 // -------------
1152 else {
1153 restoreState(epoData);
1154 break;
1155 }
1156
1157 } // for iPhase
1158
1159 } // while selectSatellites
1160
1161 restoreState(epoData);
1162 return failure;
1163}
1164
1165// Remeber Original State Vector and Variance-Covariance Matrix
1166////////////////////////////////////////////////////////////////////////////
1167void t_pppFilter::rememberState(t_epoData* epoData) {
1168
1169 _QQ_sav = _QQ;
1170
1171 QVectorIterator<t_pppParam*> itSav(_params_sav);
1172 while (itSav.hasNext()) {
1173 t_pppParam* par = itSav.next();
1174 delete par;
1175 }
1176 _params_sav.clear();
1177
1178 QVectorIterator<t_pppParam*> it(_params);
1179 while (it.hasNext()) {
1180 t_pppParam* par = it.next();
1181 _params_sav.push_back(new t_pppParam(*par));
1182 }
1183
1184 _epoData_sav->deepCopy(epoData);
1185}
1186
1187// Restore Original State Vector and Variance-Covariance Matrix
1188////////////////////////////////////////////////////////////////////////////
1189void t_pppFilter::restoreState(t_epoData* epoData) {
1190
1191 _QQ = _QQ_sav;
1192
1193 QVectorIterator<t_pppParam*> it(_params);
1194 while (it.hasNext()) {
1195 t_pppParam* par = it.next();
1196 delete par;
1197 }
1198 _params.clear();
1199
1200 QVectorIterator<t_pppParam*> itSav(_params_sav);
1201 while (itSav.hasNext()) {
1202 t_pppParam* par = itSav.next();
1203 _params.push_back(new t_pppParam(*par));
1204 }
1205
1206 epoData->deepCopy(_epoData_sav);
1207}
1208
1209//
1210////////////////////////////////////////////////////////////////////////////
1211t_irc t_pppFilter::selectSatellites(const QString& lastOutlierPrn,
1212 QMap<QString, t_satData*>& satData) {
1213
1214 // First Call
1215 // ----------
1216 if (lastOutlierPrn.isEmpty()) {
1217 _outlierGPS.clear();
1218 _outlierGlo.clear();
1219 return success;
1220 }
1221
1222 // Second and next trials
1223 // ----------------------
1224 else {
1225
1226 if (lastOutlierPrn[0] == 'R' || lastOutlierPrn[0] == 'C') {
1227 _outlierGlo << lastOutlierPrn;
1228 }
1229
1230 // Remove all Glonass Outliers
1231 // ---------------------------
1232 QStringListIterator it(_outlierGlo);
1233 while (it.hasNext()) {
1234 QString prn = it.next();
1235 if (satData.contains(prn)) {
1236 delete satData.take(prn);
1237 }
1238 }
1239
1240 if (lastOutlierPrn[0] == 'R' || lastOutlierPrn[0] == 'C') {
1241 _outlierGPS.clear();
1242 return success;
1243 }
1244
1245 // GPS Outlier appeared for the first time - try to delete it
1246 // ----------------------------------------------------------
1247 if (_outlierGPS.indexOf(lastOutlierPrn) == -1) {
1248 _outlierGPS << lastOutlierPrn;
1249 if (satData.contains(lastOutlierPrn)) {
1250 delete satData.take(lastOutlierPrn);
1251 }
1252 return success;
1253 }
1254
1255 }
1256
1257 return failure;
1258}
1259
1260//
1261////////////////////////////////////////////////////////////////////////////
1262double lorentz(const ColumnVector& aa, const ColumnVector& bb) {
1263 return aa(1)*bb(1) + aa(2)*bb(2) + aa(3)*bb(3) - aa(4)*bb(4);
1264}
1265
1266//
1267////////////////////////////////////////////////////////////////////////////
1268void t_pppFilter::bancroft(const Matrix& BBpass, ColumnVector& pos) {
1269
1270 if (pos.Nrows() != 4) {
1271 pos.ReSize(4);
1272 }
1273 pos = 0.0;
1274
1275 for (int iter = 1; iter <= 2; iter++) {
1276 Matrix BB = BBpass;
1277 int mm = BB.Nrows();
1278 for (int ii = 1; ii <= mm; ii++) {
1279 double xx = BB(ii,1);
1280 double yy = BB(ii,2);
1281 double traveltime = 0.072;
1282 if (iter > 1) {
1283 double zz = BB(ii,3);
1284 double rho = sqrt( (xx-pos(1)) * (xx-pos(1)) +
1285 (yy-pos(2)) * (yy-pos(2)) +
1286 (zz-pos(3)) * (zz-pos(3)) );
1287 traveltime = rho / t_CST::c;
1288 }
1289 double angle = traveltime * t_CST::omega;
1290 double cosa = cos(angle);
1291 double sina = sin(angle);
1292 BB(ii,1) = cosa * xx + sina * yy;
1293 BB(ii,2) = -sina * xx + cosa * yy;
1294 }
1295
1296 Matrix BBB;
1297 if (mm > 4) {
1298 SymmetricMatrix hlp; hlp << BB.t() * BB;
1299 BBB = hlp.i() * BB.t();
1300 }
1301 else {
1302 BBB = BB.i();
1303 }
1304 ColumnVector ee(mm); ee = 1.0;
1305 ColumnVector alpha(mm); alpha = 0.0;
1306 for (int ii = 1; ii <= mm; ii++) {
1307 alpha(ii) = lorentz(BB.Row(ii).t(),BB.Row(ii).t())/2.0;
1308 }
1309 ColumnVector BBBe = BBB * ee;
1310 ColumnVector BBBalpha = BBB * alpha;
1311 double aa = lorentz(BBBe, BBBe);
1312 double bb = lorentz(BBBe, BBBalpha)-1;
1313 double cc = lorentz(BBBalpha, BBBalpha);
1314 double root = sqrt(bb*bb-aa*cc);
1315
1316 Matrix hlpPos(4,2);
1317 hlpPos.Column(1) = (-bb-root)/aa * BBBe + BBBalpha;
1318 hlpPos.Column(2) = (-bb+root)/aa * BBBe + BBBalpha;
1319
1320 ColumnVector omc(2);
1321 for (int pp = 1; pp <= 2; pp++) {
1322 hlpPos(4,pp) = -hlpPos(4,pp);
1323 omc(pp) = BB(1,4) -
1324 sqrt( (BB(1,1)-hlpPos(1,pp)) * (BB(1,1)-hlpPos(1,pp)) +
1325 (BB(1,2)-hlpPos(2,pp)) * (BB(1,2)-hlpPos(2,pp)) +
1326 (BB(1,3)-hlpPos(3,pp)) * (BB(1,3)-hlpPos(3,pp)) ) -
1327 hlpPos(4,pp);
1328 }
1329 if ( fabs(omc(1)) > fabs(omc(2)) ) {
1330 pos = hlpPos.Column(2);
1331 }
1332 else {
1333 pos = hlpPos.Column(1);
1334 }
1335 }
1336}
1337
1338//
1339////////////////////////////////////////////////////////////////////////////
1340void t_pppFilter::cmpDOP(t_epoData* epoData) {
1341
1342 Tracer tracer("t_pppFilter::cmpDOP");
1343
1344 _numSat = 0;
1345 _hDop = 0.0;
1346
1347 if (_params.size() < 4) {
1348 return;
1349 }
1350
1351 const unsigned numPar = 4;
1352 Matrix AA(epoData->sizeAll(), numPar);
1353 QMapIterator<QString, t_satData*> it(epoData->satData);
1354 while (it.hasNext()) {
1355 it.next();
1356 t_satData* satData = it.value();
1357 _numSat += 1;
1358 for (unsigned iPar = 0; iPar < numPar; iPar++) {
1359 AA[_numSat-1][iPar] = _params[iPar]->partial(satData, false);
1360 }
1361 }
1362 if (_numSat < 4) {
1363 return;
1364 }
1365 AA = AA.Rows(1, _numSat);
1366 SymmetricMatrix NN; NN << AA.t() * AA;
1367 SymmetricMatrix QQ = NN.i();
1368
1369 _hDop = sqrt(QQ(1,1) + QQ(2,2));
1370}
Note: See TracBrowser for help on using the repository browser.