source: ntrip/branches/BNC_2.12/src/PPP_SSR_I/pppFilter.cpp @ 7958

Last change on this file since 7958 was 7958, checked in by stuerze, 3 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.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      = 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    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).toAscii().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 (!nObs) {
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).toAscii().data() << ' ';
1135            }
1136            QStringListIterator itGlo(_outlierGlo);
1137            while (itGlo.hasNext()) {
1138              QString prn = itGlo.next();
1139              LOG << prn.mid(0,3).toAscii().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.