1 | #include <iostream>
|
---|
2 | #include <iomanip>
|
---|
3 | #include <cmath>
|
---|
4 | #include <newmat.h>
|
---|
5 | #include <newmatio.h>
|
---|
6 |
|
---|
7 | #include "filter.h"
|
---|
8 | #include "parlist.h"
|
---|
9 | #include "ambres.h"
|
---|
10 | #include "obspool.h"
|
---|
11 | #include "kalman.h"
|
---|
12 | #include "station.h"
|
---|
13 |
|
---|
14 | using namespace BNC;
|
---|
15 | using namespace std;
|
---|
16 |
|
---|
17 | // Constructor
|
---|
18 | ////////////////////////////////////////////////////////////////////////////
|
---|
19 | t_filter::t_filter() {
|
---|
20 | _parlist = 0;
|
---|
21 | }
|
---|
22 |
|
---|
23 | // Destructor
|
---|
24 | ////////////////////////////////////////////////////////////////////////////
|
---|
25 | t_filter::~t_filter() {
|
---|
26 | delete _parlist;
|
---|
27 | }
|
---|
28 |
|
---|
29 | // Process Single Epoch
|
---|
30 | ////////////////////////////////////////////////////////////////////////////
|
---|
31 | t_irc t_filter::processEpoch(t_obsPool* obsPool) {
|
---|
32 |
|
---|
33 | _ambFixRate = 0.0;
|
---|
34 | _numSat = 0;
|
---|
35 |
|
---|
36 | if (!_parlist) {
|
---|
37 | _parlist = new t_parlist();
|
---|
38 | }
|
---|
39 |
|
---|
40 | // Vector of all Observations
|
---|
41 | // --------------------------
|
---|
42 | t_obsPool::t_epoch* epoch = obsPool->lastEpoch();
|
---|
43 | if (!epoch) {
|
---|
44 | return t_irc::failure;
|
---|
45 | }
|
---|
46 | vector<t_satObs*>& obsVector = epoch->obsVector();
|
---|
47 |
|
---|
48 | // Time of the Epoch
|
---|
49 | // -----------------
|
---|
50 | _epoTime = epoch->epoTime();
|
---|
51 |
|
---|
52 | // Auxiliary vectors of processed linear combinations
|
---|
53 | // --------------------------------------------------
|
---|
54 | vector<t_lc::type> LCsCode;
|
---|
55 | vector<t_lc::type> LCsPhase;
|
---|
56 | for (unsigned ii = 0; ii < OPT->LCs().size(); ii++) {
|
---|
57 | const t_lc::type& tLC = OPT->LCs()[ii];
|
---|
58 | if (t_lc::includesCode(tLC) && !t_lc::includesPhase(tLC)) {
|
---|
59 | LCsCode.push_back(tLC);
|
---|
60 | }
|
---|
61 | else {
|
---|
62 | LCsPhase.push_back(tLC);
|
---|
63 | }
|
---|
64 | }
|
---|
65 | vector<t_lc::type> ambLCs;
|
---|
66 | if (LCsPhase.size() == 1) {
|
---|
67 | ambLCs.push_back(LCsPhase[0]);
|
---|
68 | }
|
---|
69 | else if (LCsPhase.size() > 1) {
|
---|
70 | ambLCs.push_back(t_lc::l1);
|
---|
71 | ambLCs.push_back(t_lc::l2);
|
---|
72 | }
|
---|
73 |
|
---|
74 | // Set Parameters
|
---|
75 | // --------------
|
---|
76 | _parlist->set(_epoTime, ambLCs, obsVector);
|
---|
77 | const vector<t_param*>& params = _parlist->params();
|
---|
78 |
|
---|
79 | // Status Vector, Variance-Covariance Matrix
|
---|
80 | // -----------------------------------------
|
---|
81 | ColumnVector xFltOld = _xFlt;
|
---|
82 | SymmetricMatrix QFltOld = _QFlt;
|
---|
83 |
|
---|
84 | _QFlt.ReSize(_parlist->nPar()); _QFlt = 0.0;
|
---|
85 | _xFlt.ReSize(_parlist->nPar()); _xFlt = 0.0;
|
---|
86 | _x0.ReSize(_parlist->nPar()); _x0 = 0.0;
|
---|
87 |
|
---|
88 | for (unsigned ii = 0; ii < params.size(); ii++) {
|
---|
89 | const t_param* par1 = params[ii];
|
---|
90 |
|
---|
91 | _x0[ii] = par1->x0();
|
---|
92 |
|
---|
93 | int iOld = par1->indexOld();
|
---|
94 | if (iOld < 0) {
|
---|
95 | _QFlt[ii][ii] = par1->sigma0() * par1->sigma0(); // new parameter
|
---|
96 | }
|
---|
97 | else {
|
---|
98 | _QFlt[ii][ii] = QFltOld[iOld][iOld] + par1->noise() * par1->noise();
|
---|
99 | _xFlt[ii] = xFltOld[iOld];
|
---|
100 | for (unsigned jj = 0; jj < ii; jj++) {
|
---|
101 | const t_param* par2 = params[jj];
|
---|
102 | int jOld = par2->indexOld();
|
---|
103 | if (jOld >= 0) {
|
---|
104 | _QFlt[ii][jj] = QFltOld(iOld+1,jOld+1);
|
---|
105 | }
|
---|
106 | }
|
---|
107 | }
|
---|
108 | }
|
---|
109 |
|
---|
110 | // Process LCs containing code separately
|
---|
111 | // --------------------------------------
|
---|
112 | for (unsigned ipc = 0; ipc <= 1; ipc++) {
|
---|
113 | const vector<t_lc::type>& LCsHlp = (ipc == 0 ? LCsCode : LCsPhase);
|
---|
114 | if (LCsHlp.size() > 0) {
|
---|
115 | if ( processLC(LCsHlp, obsVector) != t_irc::success ) {
|
---|
116 | return t_irc::failure;
|
---|
117 | }
|
---|
118 | }
|
---|
119 | }
|
---|
120 |
|
---|
121 | // Copy Float Solution
|
---|
122 | // -------------------
|
---|
123 | _QFix = _QFlt;
|
---|
124 | _xFix = _xFlt;
|
---|
125 |
|
---|
126 | _ambFixRate = 0.0;
|
---|
127 |
|
---|
128 | // Constrain ZD Ambiguities
|
---|
129 | // ------------------------
|
---|
130 | if (LCsPhase.size() > 0) {
|
---|
131 | Matrix HH(LCsPhase.size(), params.size()); HH = 0.0;
|
---|
132 | ColumnVector hh(LCsPhase.size()); hh = 0.0;
|
---|
133 | DiagonalMatrix Sl(LCsPhase.size()); Sl = 1.e-4;
|
---|
134 | for (unsigned iLC = 0; iLC < LCsPhase.size(); iLC++) {
|
---|
135 | const t_param* ambPar = 0;
|
---|
136 | for (unsigned iPar = 0; iPar < params.size(); iPar++) {
|
---|
137 | const t_param* par = params[iPar];
|
---|
138 | if (par->type() == t_param::amb && par->tLC() == LCsPhase[iLC] &&
|
---|
139 | par->indexOld() != -1 && !par->ambResetCandidate()) {
|
---|
140 | if (ambPar == 0 || ambPar->ambEleSat() < par->ambEleSat()) {
|
---|
141 | ambPar = par;
|
---|
142 | }
|
---|
143 | }
|
---|
144 | }
|
---|
145 | if (ambPar) {
|
---|
146 | HH[iLC][ambPar->indexNew()] = 1.0;
|
---|
147 | hh[iLC] = xFltOld[ambPar->indexOld()];
|
---|
148 | }
|
---|
149 | }
|
---|
150 | kalman(_QFix, _xFix, HH, hh, Sl);
|
---|
151 | }
|
---|
152 |
|
---|
153 | // Ambiguity Resolution
|
---|
154 | // --------------------
|
---|
155 | if (OPT->ambres()) {
|
---|
156 | if (ambLCs.size() > 1) {
|
---|
157 | t_ambres::ambres(_epoTime, ambLCs, _parlist, _QFix, _xFix, true);
|
---|
158 | }
|
---|
159 | _ambFixRate = t_ambres::ambres(_epoTime, ambLCs, _parlist, _QFix, _xFix, false);
|
---|
160 |
|
---|
161 | // Check/Print Fixed Residuals
|
---|
162 | // ---------------------------
|
---|
163 | bool checkResOK = true;
|
---|
164 | string epoTimeStr = string(_epoTime);
|
---|
165 | for (unsigned ii = 0; ii < OPT->LCs().size(); ii++) {
|
---|
166 | const t_lc::type& tLC = OPT->LCs()[ii];
|
---|
167 | for (unsigned iObs = 0; iObs < obsVector.size(); iObs++) {
|
---|
168 | const t_satObs* obs = obsVector[iObs];
|
---|
169 | if (obs->isValid() && !obs->outlier()) {
|
---|
170 | ColumnVector AA(params.size());
|
---|
171 | for (unsigned iPar = 0; iPar < params.size(); iPar++) {
|
---|
172 | const t_param* par = params[iPar];
|
---|
173 | AA[iPar] = par->partial(_epoTime, obs, tLC);
|
---|
174 | }
|
---|
175 | double ll = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA);
|
---|
176 | double vv = DotProduct(AA, _xFix) - ll;
|
---|
177 |
|
---|
178 | bool outlier = false;
|
---|
179 | if (fabs(vv) > (t_lc::includesCode(tLC) ? 1.5 * OPT->maxResCode() : 1.5 * OPT->maxResPhase())) {
|
---|
180 | outlier = true;
|
---|
181 | checkResOK = false;
|
---|
182 | }
|
---|
183 |
|
---|
184 | if (OPT->logLevel() > 1) {
|
---|
185 | LOG << epoTimeStr << " XRES "
|
---|
186 | << left << setw(3) << t_lc::toString(tLC) << right << ' '
|
---|
187 | << obs->prn().toString() << ' '
|
---|
188 | << setw(8) << setprecision(4) << vv;
|
---|
189 | if (outlier) {
|
---|
190 | LOG << " outlier AR";
|
---|
191 | }
|
---|
192 | LOG << endl;
|
---|
193 | }
|
---|
194 | }
|
---|
195 | }
|
---|
196 | }
|
---|
197 | if (!checkResOK || _ambFixRate < OPT->ambresMinFixRate()) {
|
---|
198 | _ambFixRate = 0.0;
|
---|
199 | _xFix = _xFlt;
|
---|
200 | _QFix = _QFlt;
|
---|
201 | }
|
---|
202 | }
|
---|
203 |
|
---|
204 | _parlist->printResult(_epoTime, _QFix, _xFix, _ambFixRate);
|
---|
205 |
|
---|
206 | return t_irc::success;
|
---|
207 | }
|
---|
208 |
|
---|
209 | // Process Selected LCs
|
---|
210 | ////////////////////////////////////////////////////////////////////////////
|
---|
211 | t_irc t_filter::processLC(const vector<t_lc::type>& LCs,
|
---|
212 | vector<t_satObs*>& obsVector) {
|
---|
213 |
|
---|
214 | LOG.setf(ios::fixed);
|
---|
215 |
|
---|
216 | // Detect Cycle Slips
|
---|
217 | // ------------------
|
---|
218 | if (detectCycleSlips(LCs, obsVector) != t_irc::success) {
|
---|
219 | return t_irc::failure;
|
---|
220 | }
|
---|
221 |
|
---|
222 | ColumnVector xSav = _xFlt;
|
---|
223 | SymmetricMatrix QSav = _QFlt;
|
---|
224 | string epoTimeStr = string(_epoTime);
|
---|
225 | const vector<t_param*>& params = _parlist->params();
|
---|
226 | unsigned maxObs = obsVector.size() * LCs.size();
|
---|
227 |
|
---|
228 | // Outlier Detection Loop
|
---|
229 | // ----------------------
|
---|
230 | for (unsigned iOutlier = 0; iOutlier < maxObs; iOutlier++) {
|
---|
231 |
|
---|
232 | if (iOutlier > 0) {
|
---|
233 | _xFlt = xSav;
|
---|
234 | _QFlt = QSav;
|
---|
235 | }
|
---|
236 |
|
---|
237 | // First-Design Matrix, Terms Observed-Computed, Weight Matrix
|
---|
238 | // -----------------------------------------------------------
|
---|
239 | Matrix AA(maxObs, _parlist->nPar());
|
---|
240 | ColumnVector ll(maxObs);
|
---|
241 | UpperTriangularMatrix Sl(maxObs); Sl = 0.0;
|
---|
242 |
|
---|
243 | int iObs = -1;
|
---|
244 | vector<t_satObs*> usedObs;
|
---|
245 | vector<t_lc::type> usedTypes;
|
---|
246 | for (unsigned ii = 0; ii < obsVector.size(); ii++) {
|
---|
247 | t_satObs* obs = obsVector[ii];
|
---|
248 | if (!obs->outlier()) {
|
---|
249 | Matrix CC(LCs.size(), 4);
|
---|
250 | for (unsigned jj = 0; jj < LCs.size(); jj++) {
|
---|
251 | const t_lc::type tLC = LCs[jj];
|
---|
252 | ++iObs;
|
---|
253 | usedObs.push_back(obs);
|
---|
254 | usedTypes.push_back(tLC);
|
---|
255 | for (unsigned iPar = 0; iPar < params.size(); iPar++) {
|
---|
256 | const t_param* par = params[iPar];
|
---|
257 | AA[iObs][iPar] = par->partial(_epoTime, obs, tLC);
|
---|
258 | }
|
---|
259 | ll[iObs] = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA.Row(iObs+1));
|
---|
260 | if (LCs.size() > 1) {
|
---|
261 | ColumnVector coeff(4);
|
---|
262 | obs->lc(tLC, 0.0, 0.0, 0.0, 0.0, &coeff);
|
---|
263 | CC[jj][0] = coeff[0] * obs->sigma(t_lc::l1);
|
---|
264 | CC[jj][1] = coeff[1] * obs->sigma(t_lc::l2);
|
---|
265 | CC[jj][2] = coeff[2] * obs->sigma(t_lc::c1);
|
---|
266 | CC[jj][3] = coeff[3] * obs->sigma(t_lc::c2);
|
---|
267 | }
|
---|
268 | else {
|
---|
269 | Sl[iObs][iObs] = obs->sigma(tLC);
|
---|
270 | }
|
---|
271 | }
|
---|
272 | if (LCs.size() > 1) {
|
---|
273 | SymmetricMatrix QQ; QQ << CC * CC.t();
|
---|
274 | Sl.SymSubMatrix(iObs-LCs.size()+2, iObs+1) = Cholesky(QQ).t();
|
---|
275 | }
|
---|
276 | }
|
---|
277 | }
|
---|
278 |
|
---|
279 | // Check number of observations, truncate matrices
|
---|
280 | // -----------------------------------------------
|
---|
281 | if (iObs+1 < OPT->minobs()) {
|
---|
282 | return t_irc::failure;
|
---|
283 | }
|
---|
284 | AA = AA.Rows(1, iObs+1);
|
---|
285 | ll = ll.Rows(1, iObs+1);
|
---|
286 | Sl = Sl.SymSubMatrix(1, iObs+1);
|
---|
287 |
|
---|
288 | // Kalman update step
|
---|
289 | // ------------------
|
---|
290 | kalman(_QFlt, _xFlt, AA, ll, Sl);
|
---|
291 |
|
---|
292 | // Check Residuals
|
---|
293 | // ---------------
|
---|
294 | ColumnVector vv = AA * _xFlt - ll;
|
---|
295 | double maxOutlier = 0.0;
|
---|
296 | int maxOutlierIndex = -1;
|
---|
297 | t_lc::type maxOutlierLC = t_lc::dummy;
|
---|
298 | for (unsigned ii = 0; ii < usedObs.size(); ii++) {
|
---|
299 | const t_lc::type tLC = usedTypes[ii];
|
---|
300 | double res = fabs(vv[ii]);
|
---|
301 | if (res >
|
---|
302 | (t_lc::includesCode(tLC) ? OPT->maxResCode() : OPT->maxResPhase())) {
|
---|
303 | if (res > fabs(maxOutlier)) {
|
---|
304 | maxOutlier = vv[ii];
|
---|
305 | maxOutlierIndex = ii;
|
---|
306 | maxOutlierLC = tLC;
|
---|
307 | }
|
---|
308 | }
|
---|
309 | }
|
---|
310 |
|
---|
311 | // Mark outlier or break outlier detection loop
|
---|
312 | // --------------------------------------------
|
---|
313 | if (maxOutlierIndex > -1) {
|
---|
314 | t_satObs* obs = usedObs[maxOutlierIndex];
|
---|
315 | t_param* par = 0;
|
---|
316 | LOG << epoTimeStr << " Outlier " << t_lc::toString(maxOutlierLC) << ' '
|
---|
317 | << obs->prn().toString() << ' '
|
---|
318 | << setw(8) << setprecision(4) << maxOutlier << endl;
|
---|
319 | for (unsigned iPar = 0; iPar < params.size(); iPar++) {
|
---|
320 | t_param* hlp = params[iPar];
|
---|
321 | if (hlp->type() == t_param::amb && hlp->prn() == obs->prn() &&
|
---|
322 | hlp->tLC() == usedTypes[maxOutlierIndex]) {
|
---|
323 | par = hlp;
|
---|
324 | }
|
---|
325 | }
|
---|
326 | if (par) {
|
---|
327 | if (par->ambResetCandidate()) {
|
---|
328 | resetAmb(par->prn(), obsVector, &QSav, &xSav);
|
---|
329 | }
|
---|
330 | else {
|
---|
331 | par->setAmbResetCandidate();
|
---|
332 | obs->setOutlier();
|
---|
333 | }
|
---|
334 | }
|
---|
335 | else {
|
---|
336 | obs->setOutlier();
|
---|
337 | }
|
---|
338 | }
|
---|
339 |
|
---|
340 | // Print Residuals
|
---|
341 | // ---------------
|
---|
342 | else {
|
---|
343 | for (unsigned jj = 0; jj < LCs.size(); jj++) {
|
---|
344 | for (unsigned ii = 0; ii < usedObs.size(); ii++) {
|
---|
345 | const t_lc::type tLC = usedTypes[ii];
|
---|
346 | t_satObs* obs = usedObs[ii];
|
---|
347 | if (tLC == LCs[jj]) {
|
---|
348 | obs->setRes(tLC, vv[ii]);
|
---|
349 | if (OPT->logLevel() > 1) {
|
---|
350 | LOG << epoTimeStr << " RES "
|
---|
351 | << left << setw(3) << t_lc::toString(tLC) << right << ' '
|
---|
352 | << obs->prn().toString() << ' '
|
---|
353 | << setw(8) << setprecision(4) << vv[ii] << endl;
|
---|
354 | }
|
---|
355 | }
|
---|
356 | }
|
---|
357 | }
|
---|
358 | cmpDOP(LCs, AA);
|
---|
359 | break;
|
---|
360 | }
|
---|
361 | }
|
---|
362 |
|
---|
363 | return t_irc::success;
|
---|
364 | }
|
---|
365 |
|
---|
366 | // Cycle-Slip Detection
|
---|
367 | ////////////////////////////////////////////////////////////////////////////
|
---|
368 | t_irc t_filter::detectCycleSlips(const vector<t_lc::type>& LCs,
|
---|
369 | const vector<t_satObs*>& obsVector) {
|
---|
370 |
|
---|
371 | const double SLIP = 20.0; // slip threshold
|
---|
372 | string epoTimeStr = string(_epoTime);
|
---|
373 | const vector<t_param*>& params = _parlist->params();
|
---|
374 |
|
---|
375 | for (unsigned ii = 0; ii < LCs.size(); ii++) {
|
---|
376 | const t_lc::type& tLC = LCs[ii];
|
---|
377 | if (t_lc::includesPhase(tLC)) {
|
---|
378 | for (unsigned iObs = 0; iObs < obsVector.size(); iObs++) {
|
---|
379 | const t_satObs* obs = obsVector[iObs];
|
---|
380 |
|
---|
381 | // Check set Slips and Jump Counters
|
---|
382 | // ---------------------------------
|
---|
383 | bool slip = false;
|
---|
384 |
|
---|
385 | if (obs->slip()) {
|
---|
386 | LOG << "cycle slip set (obs)" << endl;;
|
---|
387 | slip = true;
|
---|
388 | }
|
---|
389 |
|
---|
390 | if (_slips[obs->prn()]._obsSlipCounter != -1 &&
|
---|
391 | _slips[obs->prn()]._obsSlipCounter != obs->slipCounter()) {
|
---|
392 | LOG << "cycle slip set (obsSlipCounter)" << endl;
|
---|
393 | slip = true;
|
---|
394 | }
|
---|
395 | _slips[obs->prn()]._obsSlipCounter = obs->slipCounter();
|
---|
396 |
|
---|
397 | if (_slips[obs->prn()]._biasJumpCounter != -1 &&
|
---|
398 | _slips[obs->prn()]._biasJumpCounter != obs->biasJumpCounter()) {
|
---|
399 | LOG << "cycle slip set (biasJumpCounter)" << endl;
|
---|
400 | slip = true;
|
---|
401 | }
|
---|
402 | _slips[obs->prn()]._biasJumpCounter = obs->biasJumpCounter();
|
---|
403 |
|
---|
404 | // Slip Set
|
---|
405 | // --------
|
---|
406 | if (slip) {
|
---|
407 | resetAmb(obs->prn(), obsVector);
|
---|
408 | }
|
---|
409 |
|
---|
410 | // Check Pre-Fit Residuals
|
---|
411 | // -----------------------
|
---|
412 | else {
|
---|
413 | ColumnVector AA(params.size());
|
---|
414 | for (unsigned iPar = 0; iPar < params.size(); iPar++) {
|
---|
415 | const t_param* par = params[iPar];
|
---|
416 | AA[iPar] = par->partial(_epoTime, obs, tLC);
|
---|
417 | }
|
---|
418 |
|
---|
419 | double ll = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA);
|
---|
420 | double vv = DotProduct(AA, _xFlt) - ll;
|
---|
421 |
|
---|
422 | if (fabs(vv) > SLIP) {
|
---|
423 | LOG << epoTimeStr << " cycle slip detected " << t_lc::toString(tLC) << ' '
|
---|
424 | << obs->prn().toString() << ' ' << setw(8) << setprecision(4) << vv << endl;
|
---|
425 | resetAmb(obs->prn(), obsVector);
|
---|
426 | }
|
---|
427 | }
|
---|
428 | }
|
---|
429 | }
|
---|
430 | }
|
---|
431 |
|
---|
432 | return t_irc::success;
|
---|
433 | }
|
---|
434 |
|
---|
435 | // Reset Ambiguity Parameter (cycle slip)
|
---|
436 | ////////////////////////////////////////////////////////////////////////////
|
---|
437 | t_irc t_filter::resetAmb(t_prn prn, const vector<t_satObs*>& obsVector,
|
---|
438 | SymmetricMatrix* QSav, ColumnVector* xSav) {
|
---|
439 | t_irc irc = t_irc::failure;
|
---|
440 | vector<t_param*>& params = _parlist->params();
|
---|
441 | for (unsigned iPar = 0; iPar < params.size(); iPar++) {
|
---|
442 | t_param* par = params[iPar];
|
---|
443 | if (par->type() == t_param::amb && par->prn() == prn) {
|
---|
444 | int ind = par->indexNew();
|
---|
445 | t_lc::type tLC = par->tLC();
|
---|
446 | LOG << string(_epoTime) << " RESET " << par->toString() << endl;
|
---|
447 | delete par; par = new t_param(t_param::amb, prn, tLC, &obsVector);
|
---|
448 | par->setIndex(ind);
|
---|
449 | params[iPar] = par;
|
---|
450 | for (unsigned ii = 1; ii <= params.size(); ii++) {
|
---|
451 | _QFlt(ii, ind+1) = 0.0;
|
---|
452 | if (QSav) {
|
---|
453 | (*QSav)(ii, ind+1) = 0.0;
|
---|
454 | }
|
---|
455 | }
|
---|
456 | _QFlt(ind+1,ind+1) = par->sigma0() * par->sigma0();
|
---|
457 | if (QSav) {
|
---|
458 | (*QSav)(ind+1,ind+1) = _QFlt(ind+1,ind+1);
|
---|
459 | }
|
---|
460 | _xFlt[ind] = 0.0;
|
---|
461 | if (xSav) {
|
---|
462 | (*xSav)[ind] = _xFlt[ind];
|
---|
463 | }
|
---|
464 | _x0[ind] = par->x0();
|
---|
465 | irc = t_irc::success;
|
---|
466 | }
|
---|
467 | }
|
---|
468 |
|
---|
469 | return irc;
|
---|
470 | }
|
---|
471 |
|
---|
472 | // Compute various DOP Values
|
---|
473 | ////////////////////////////////////////////////////////////////////////////
|
---|
474 | void t_filter::cmpDOP(const std::vector<t_lc::type>& LCs, const Matrix& AA) {
|
---|
475 |
|
---|
476 | _dop.reset();
|
---|
477 | _numSat = 0;
|
---|
478 | try {
|
---|
479 | _numSat = AA.Nrows() / LCs.size();
|
---|
480 |
|
---|
481 | if (_numSat < 4) {
|
---|
482 | return;
|
---|
483 | }
|
---|
484 |
|
---|
485 | Matrix BB(_numSat, 4);
|
---|
486 |
|
---|
487 | for (int ii = 1; ii <= _numSat; ii++) {
|
---|
488 | BB.Row(ii) = AA.Row(ii*LCs.size()).columns(1,4);
|
---|
489 | }
|
---|
490 |
|
---|
491 | SymmetricMatrix NN; NN << BB.t() * BB;
|
---|
492 | SymmetricMatrix QQ = NN.i();
|
---|
493 |
|
---|
494 | _dop.P = sqrt(QQ(1,1) + QQ(2,2) + QQ(3,3));
|
---|
495 | _dop.T = sqrt(QQ(4,4));
|
---|
496 | _dop.G = sqrt(QQ(1,1) + QQ(2,2) + QQ(3,3) + QQ(4,4));
|
---|
497 | }
|
---|
498 | catch (...) {
|
---|
499 | }
|
---|
500 | }
|
---|