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

Last change on this file since 7545 was 7545, checked in by stuerze, 7 years ago

minor changes regarding SATNUM output

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