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

Last change on this file since 7955 was 7955, checked in by stuerze, 4 years 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.8 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      = 2.98 * 0.03;
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 (isnan(_xcBanc(1)) ||
298      isnan(_xcBanc(2)) ||
299      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).toAscii().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).toAscii().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).toAscii().data() << ' ' << maxResGlo << endl;
760      return prnGlo;
761    }
762    else if (maxResGPS > 2.98 * OPT->_maxResL1) {
763      LOG << "Outlier Phase " << prnGPS.mid(0,3).toAscii().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).toAscii().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    if (satData->system() == 'R') {
944      sigP3 *= GLONASS_WEIGHT_FACTOR;
945    }
946    ll(iObs)      = satData->P3 - cmpValue(satData, false);
947    PP(iObs,iObs) = 1.0 / (sigP3 * sigP3) / (ellWgtCoef * ellWgtCoef);
948    for (int iPar = 1; iPar <= _params.size(); iPar++) {
949      AA(iObs, iPar) = _params[iPar-1]->partial(satData, false);
950    }
951  }
952}
953
954//
955///////////////////////////////////////////////////////////////////////////
956QByteArray t_pppFilter::printRes(int iPhase, const ColumnVector& vv,
957                              const QMap<QString, t_satData*>& satDataMap) {
958
959  Tracer tracer("t_pppFilter::printRes");
960
961  ostringstream str;
962  str.setf(ios::fixed);
963  bool useObs;
964  QMapIterator<QString, t_satData*> it(satDataMap);
965  while (it.hasNext()) {
966    it.next();
967    t_satData* satData = it.value();
968    (iPhase == 0) ? useObs = OPT->codeLCs(satData->system()).size() :
969                    useObs = OPT->ambLCs(satData->system()).size();
970    if (satData->obsIndex != 0 && useObs) {
971      str << _time.datestr() << "_" << _time.timestr(3)
972          << " RES " << satData->prn.mid(0,3).toAscii().data()
973          << (iPhase ? "   L3 " : "   P3 ")
974          << setw(9) << setprecision(4) << vv(satData->obsIndex) << endl;
975    }
976  }
977
978  return QByteArray(str.str().c_str());
979}
980
981//
982///////////////////////////////////////////////////////////////////////////
983void t_pppFilter::findMaxRes(const ColumnVector& vv,
984                          const QMap<QString, t_satData*>& satData,
985                          QString& prnGPS, QString& prnGlo,
986                          double& maxResGPS, double& maxResGlo) {
987
988  Tracer tracer("t_pppFilter::findMaxRes");
989
990  maxResGPS  = 0.0;
991  maxResGlo  = 0.0;
992
993  QMapIterator<QString, t_satData*> it(satData);
994  while (it.hasNext()) {
995    it.next();
996    t_satData* satData = it.value();
997    if (satData->obsIndex != 0) {
998      QString prn = satData->prn;
999      if (prn[0] == 'R' || prn[0] == 'C') {
1000        if (fabs(vv(satData->obsIndex)) > maxResGlo) {
1001          maxResGlo = fabs(vv(satData->obsIndex));
1002          prnGlo    = prn;
1003        }
1004      }
1005      else {
1006        if (fabs(vv(satData->obsIndex)) > maxResGPS) {
1007          maxResGPS = fabs(vv(satData->obsIndex));
1008          prnGPS    = prn;
1009        }
1010      }
1011    }
1012  }
1013}
1014
1015// Update Step (private - loop over outliers)
1016////////////////////////////////////////////////////////////////////////////
1017t_irc t_pppFilter::update_p(t_epoData* epoData) {
1018
1019  Tracer tracer("t_pppFilter::update_p");
1020
1021  // Save Variance-Covariance Matrix, and Status Vector
1022  // --------------------------------------------------
1023  rememberState(epoData);
1024
1025  QString lastOutlierPrn;
1026
1027  // Try with all satellites, then with all minus one, etc.
1028  // ------------------------------------------------------
1029  while (selectSatellites(lastOutlierPrn, epoData->satData) == success) {
1030
1031    QByteArray strResCode;
1032    QByteArray strResPhase;
1033
1034    // Bancroft Solution
1035    // -----------------
1036    if (cmpBancroft(epoData) != success) {
1037      break;
1038    }
1039
1040    // First update using code observations, then phase observations
1041    // -------------------------------------------------------------
1042    bool usePhase = OPT->ambLCs('G').size() || OPT->ambLCs('R').size() ||
1043                    OPT->ambLCs('E').size() || OPT->ambLCs('C').size() ;
1044
1045    char sys[] ={'G', 'R', 'E', 'C'};
1046
1047    bool satnumPrinted[] = {false, false, false, false};
1048
1049    for (int iPhase = 0; iPhase <= (usePhase ? 1 : 0); iPhase++) {
1050
1051      // Status Prediction
1052      // -----------------
1053      predict(iPhase, epoData);
1054
1055      // Create First-Design Matrix
1056      // --------------------------
1057      unsigned nPar = _params.size();
1058      unsigned nObs = 0;
1059      nObs = epoData->sizeAll();
1060      bool useObs = false;
1061      for (unsigned ii = 0; ii < sizeof(sys); ii++) {
1062        const char s = sys[ii];
1063        (iPhase == 0) ? useObs = OPT->codeLCs(s).size() : useObs = OPT->ambLCs(s).size();
1064        if (!useObs) {
1065          nObs -= epoData->sizeSys(s);
1066        }
1067        else {
1068          if (!satnumPrinted[ii]) {
1069            satnumPrinted[ii] = true;
1070            LOG << _time.datestr() << "_" << _time.timestr(3)
1071                << " SATNUM " << s << ' ' << right << setw(2)
1072                << epoData->sizeSys(s) << endl;
1073          }
1074        }
1075      }
1076
1077      if (!nObs) {
1078        restoreState(epoData);
1079        return failure;
1080      }
1081
1082      // Prepare first-design Matrix, vector observed-computed
1083      // -----------------------------------------------------
1084      Matrix          AA(nObs, nPar);  // first design matrix
1085      ColumnVector    ll(nObs);        // terms observed-computed
1086      DiagonalMatrix  PP(nObs); PP = 0.0;
1087
1088      unsigned iObs = 0;
1089      QMapIterator<QString, t_satData*> it(epoData->satData);
1090
1091      while (it.hasNext()) {
1092        it.next();
1093        t_satData* satData = it.value();
1094        QString prn = satData->prn;
1095        (iPhase == 0) ? useObs = OPT->codeLCs(satData->system()).size() :
1096                        useObs = OPT->ambLCs(satData->system()).size();
1097        if (useObs) {
1098          addObs(iPhase, iObs, satData, AA, ll, PP);
1099        } else {
1100          satData->obsIndex = 0;
1101        }
1102      }
1103
1104      // Compute Filter Update
1105      // ---------------------
1106      ColumnVector dx(nPar); dx = 0.0;
1107      kalman(AA, ll, PP, _QQ, dx);
1108      ColumnVector vv = ll - AA * dx;
1109
1110      // Print Residuals
1111      // ---------------
1112      if (iPhase == 0) {
1113        strResCode  = printRes(iPhase, vv, epoData->satData);
1114      }
1115      else {
1116        strResPhase = printRes(iPhase, vv, epoData->satData);
1117      }
1118
1119      // Check the residuals
1120      // -------------------
1121      lastOutlierPrn = outlierDetection(iPhase, vv, epoData->satData);
1122
1123      // No Outlier Detected
1124      // -------------------
1125      if (lastOutlierPrn.isEmpty()) {
1126
1127        QVectorIterator<t_pppParam*> itPar(_params);
1128        while (itPar.hasNext()) {
1129          t_pppParam* par = itPar.next();
1130          par->xx += dx(par->index);
1131        }
1132
1133        if (!usePhase || iPhase == 1) {
1134          if (_outlierGPS.size() > 0 || _outlierGlo.size() > 0) {
1135            LOG << "Neglected PRNs: ";
1136            if (!_outlierGPS.isEmpty()) {
1137              LOG << _outlierGPS.last().mid(0,3).toAscii().data() << ' ';
1138            }
1139            QStringListIterator itGlo(_outlierGlo);
1140            while (itGlo.hasNext()) {
1141              QString prn = itGlo.next();
1142              LOG << prn.mid(0,3).toAscii().data() << ' ';
1143            }
1144            LOG << endl;
1145          }
1146          LOG << strResCode.data() << strResPhase.data();
1147
1148          return success;
1149        }
1150      }
1151
1152      // Outlier Found
1153      // -------------
1154      else {
1155        restoreState(epoData);
1156        break;
1157      }
1158
1159    } // for iPhase
1160
1161  } // while selectSatellites
1162
1163  restoreState(epoData);
1164  return failure;
1165}
1166
1167// Remeber Original State Vector and Variance-Covariance Matrix
1168////////////////////////////////////////////////////////////////////////////
1169void t_pppFilter::rememberState(t_epoData* epoData) {
1170
1171  _QQ_sav = _QQ;
1172
1173  QVectorIterator<t_pppParam*> itSav(_params_sav);
1174  while (itSav.hasNext()) {
1175    t_pppParam* par = itSav.next();
1176    delete par;
1177  }
1178  _params_sav.clear();
1179
1180  QVectorIterator<t_pppParam*> it(_params);
1181  while (it.hasNext()) {
1182    t_pppParam* par = it.next();
1183    _params_sav.push_back(new t_pppParam(*par));
1184  }
1185
1186  _epoData_sav->deepCopy(epoData);
1187}
1188
1189// Restore Original State Vector and Variance-Covariance Matrix
1190////////////////////////////////////////////////////////////////////////////
1191void t_pppFilter::restoreState(t_epoData* epoData) {
1192
1193  _QQ = _QQ_sav;
1194
1195  QVectorIterator<t_pppParam*> it(_params);
1196  while (it.hasNext()) {
1197    t_pppParam* par = it.next();
1198    delete par;
1199  }
1200  _params.clear();
1201
1202  QVectorIterator<t_pppParam*> itSav(_params_sav);
1203  while (itSav.hasNext()) {
1204    t_pppParam* par = itSav.next();
1205    _params.push_back(new t_pppParam(*par));
1206  }
1207
1208  epoData->deepCopy(_epoData_sav);
1209}
1210
1211//
1212////////////////////////////////////////////////////////////////////////////
1213t_irc t_pppFilter::selectSatellites(const QString& lastOutlierPrn,
1214                                 QMap<QString, t_satData*>& satData) {
1215
1216  // First Call
1217  // ----------
1218  if (lastOutlierPrn.isEmpty()) {
1219    _outlierGPS.clear();
1220    _outlierGlo.clear();
1221    return success;
1222  }
1223
1224  // Second and next trials
1225  // ----------------------
1226  else {
1227
1228    if (lastOutlierPrn[0] == 'R' || lastOutlierPrn[0] == 'C') {
1229      _outlierGlo << lastOutlierPrn;
1230    }
1231
1232    // Remove all Glonass Outliers
1233    // ---------------------------
1234    QStringListIterator it(_outlierGlo);
1235    while (it.hasNext()) {
1236      QString prn = it.next();
1237      if (satData.contains(prn)) {
1238        delete satData.take(prn);
1239      }
1240    }
1241
1242    if (lastOutlierPrn[0] == 'R' || lastOutlierPrn[0] == 'C') {
1243      _outlierGPS.clear();
1244      return success;
1245    }
1246
1247    // GPS Outlier appeared for the first time - try to delete it
1248    // ----------------------------------------------------------
1249    if (_outlierGPS.indexOf(lastOutlierPrn) == -1) {
1250      _outlierGPS << lastOutlierPrn;
1251      if (satData.contains(lastOutlierPrn)) {
1252        delete satData.take(lastOutlierPrn);
1253      }
1254      return success;
1255    }
1256
1257  }
1258
1259  return failure;
1260}
1261
1262//
1263////////////////////////////////////////////////////////////////////////////
1264double lorentz(const ColumnVector& aa, const ColumnVector& bb) {
1265  return aa(1)*bb(1) +  aa(2)*bb(2) +  aa(3)*bb(3) -  aa(4)*bb(4);
1266}
1267
1268//
1269////////////////////////////////////////////////////////////////////////////
1270void t_pppFilter::bancroft(const Matrix& BBpass, ColumnVector& pos) {
1271
1272  if (pos.Nrows() != 4) {
1273    pos.ReSize(4);
1274  }
1275  pos = 0.0;
1276
1277  for (int iter = 1; iter <= 2; iter++) {
1278    Matrix BB = BBpass;
1279    int mm = BB.Nrows();
1280    for (int ii = 1; ii <= mm; ii++) {
1281      double xx = BB(ii,1);
1282      double yy = BB(ii,2);
1283      double traveltime = 0.072;
1284      if (iter > 1) {
1285        double zz  = BB(ii,3);
1286        double rho = sqrt( (xx-pos(1)) * (xx-pos(1)) +
1287                           (yy-pos(2)) * (yy-pos(2)) +
1288                           (zz-pos(3)) * (zz-pos(3)) );
1289        traveltime = rho / t_CST::c;
1290      }
1291      double angle = traveltime * t_CST::omega;
1292      double cosa  = cos(angle);
1293      double sina  = sin(angle);
1294      BB(ii,1) =  cosa * xx + sina * yy;
1295      BB(ii,2) = -sina * xx + cosa * yy;
1296    }
1297
1298    Matrix BBB;
1299    if (mm > 4) {
1300      SymmetricMatrix hlp; hlp << BB.t() * BB;
1301      BBB = hlp.i() * BB.t();
1302    }
1303    else {
1304      BBB = BB.i();
1305    }
1306    ColumnVector ee(mm); ee = 1.0;
1307    ColumnVector alpha(mm); alpha = 0.0;
1308    for (int ii = 1; ii <= mm; ii++) {
1309      alpha(ii) = lorentz(BB.Row(ii).t(),BB.Row(ii).t())/2.0;
1310    }
1311    ColumnVector BBBe     = BBB * ee;
1312    ColumnVector BBBalpha = BBB * alpha;
1313    double aa = lorentz(BBBe, BBBe);
1314    double bb = lorentz(BBBe, BBBalpha)-1;
1315    double cc = lorentz(BBBalpha, BBBalpha);
1316    double root = sqrt(bb*bb-aa*cc);
1317
1318    Matrix hlpPos(4,2);
1319    hlpPos.Column(1) = (-bb-root)/aa * BBBe + BBBalpha;
1320    hlpPos.Column(2) = (-bb+root)/aa * BBBe + BBBalpha;
1321
1322    ColumnVector omc(2);
1323    for (int pp = 1; pp <= 2; pp++) {
1324      hlpPos(4,pp)      = -hlpPos(4,pp);
1325      omc(pp) = BB(1,4) -
1326                sqrt( (BB(1,1)-hlpPos(1,pp)) * (BB(1,1)-hlpPos(1,pp)) +
1327                      (BB(1,2)-hlpPos(2,pp)) * (BB(1,2)-hlpPos(2,pp)) +
1328                      (BB(1,3)-hlpPos(3,pp)) * (BB(1,3)-hlpPos(3,pp)) ) -
1329                hlpPos(4,pp);
1330    }
1331    if ( fabs(omc(1)) > fabs(omc(2)) ) {
1332      pos = hlpPos.Column(2);
1333    }
1334    else {
1335      pos = hlpPos.Column(1);
1336    }
1337  }
1338}
1339
1340//
1341////////////////////////////////////////////////////////////////////////////
1342void t_pppFilter::cmpDOP(t_epoData* epoData) {
1343
1344  Tracer tracer("t_pppFilter::cmpDOP");
1345
1346  _numSat = 0;
1347  _hDop   = 0.0;
1348
1349  if (_params.size() < 4) {
1350    return;
1351  }
1352
1353  const unsigned numPar = 4;
1354  Matrix AA(epoData->sizeAll(), numPar);
1355  QMapIterator<QString, t_satData*> it(epoData->satData);
1356  while (it.hasNext()) {
1357    it.next();
1358    t_satData* satData = it.value();
1359    _numSat += 1;
1360    for (unsigned iPar = 0; iPar < numPar; iPar++) {
1361      AA[_numSat-1][iPar] = _params[iPar]->partial(satData, false);
1362    }
1363  }
1364  if (_numSat < 4) {
1365    return;
1366  }
1367  AA = AA.Rows(1, _numSat);
1368  SymmetricMatrix NN; NN << AA.t() * AA;
1369  SymmetricMatrix QQ = NN.i();
1370
1371  _hDop = sqrt(QQ(1,1) + QQ(2,2));
1372}
Note: See TracBrowser for help on using the repository browser.