1 | #include <cmath>
|
---|
2 | #include <iostream>
|
---|
3 | #include <sstream>
|
---|
4 | #include <iomanip>
|
---|
5 | #include <algorithm>
|
---|
6 | #include <newmatio.h>
|
---|
7 | #include "parlist.h"
|
---|
8 | #include "satobs.h"
|
---|
9 | #include "station.h"
|
---|
10 | #include "tropo.h"
|
---|
11 | #include "iono.h"
|
---|
12 | #include "utils.h"
|
---|
13 | #include "genconst.h"
|
---|
14 |
|
---|
15 | using namespace BNC;
|
---|
16 | using namespace std;
|
---|
17 |
|
---|
18 | // Constructor
|
---|
19 | ////////////////////////////////////////////////////////////////////////////
|
---|
20 | t_param::t_param(e_type type, const t_prn& prn, t_lc::type tLC,
|
---|
21 | const vector<t_satObs*>* obsVector) {
|
---|
22 |
|
---|
23 | _type = type;
|
---|
24 | _prn = prn;
|
---|
25 | _tLC = tLC;
|
---|
26 | _x0 = 0.0;
|
---|
27 | _indexOld = -1;
|
---|
28 | _indexNew = -1;
|
---|
29 | _noise = 0.0;
|
---|
30 | _ambInfo = 0;
|
---|
31 |
|
---|
32 | switch (_type) {
|
---|
33 | case crdX:
|
---|
34 | _epoSpec = true;
|
---|
35 | _sigma0 = OPT->sigmaCrd();
|
---|
36 | break;
|
---|
37 | case crdY:
|
---|
38 | _epoSpec = true;
|
---|
39 | _sigma0 = OPT->sigmaCrd();
|
---|
40 | break;
|
---|
41 | case crdZ:
|
---|
42 | _epoSpec = true;
|
---|
43 | _sigma0 = OPT->sigmaCrd();
|
---|
44 | break;
|
---|
45 | case clkR:
|
---|
46 | _epoSpec = true;
|
---|
47 | _sigma0 = 1000.0;
|
---|
48 | break;
|
---|
49 | case amb:
|
---|
50 | _ambInfo = new t_ambInfo();
|
---|
51 | if (obsVector) {
|
---|
52 | for (unsigned ii = 0; ii < obsVector->size(); ii++) {
|
---|
53 | const t_satObs* obs = obsVector->at(ii);
|
---|
54 | if (obs->prn() == _prn) {
|
---|
55 | double offGG = 0;
|
---|
56 | if (_prn.system() == 'R' && tLC != t_lc::MW) {
|
---|
57 | offGG = PPP_CLIENT->offGG();
|
---|
58 | }
|
---|
59 | _x0 = nint((obs->obsValue(tLC) - offGG - obs->cmpValue(tLC)) / obs->lambda(tLC));
|
---|
60 | break;
|
---|
61 | }
|
---|
62 | }
|
---|
63 | }
|
---|
64 | _epoSpec = false;
|
---|
65 | _sigma0 = 100.0;
|
---|
66 | break;
|
---|
67 | case offGG:
|
---|
68 | _epoSpec = true;
|
---|
69 | _sigma0 = 1000.0;
|
---|
70 | _x0 = PPP_CLIENT->offGG();
|
---|
71 | break;
|
---|
72 | case trp:
|
---|
73 | _epoSpec = false;
|
---|
74 | _sigma0 = OPT->sigmaTropo();
|
---|
75 | _noise = OPT->noiseTropo();
|
---|
76 | break;
|
---|
77 | case iono:
|
---|
78 | if (OPT->noiseIono() > 0.0) {
|
---|
79 | _epoSpec = false;
|
---|
80 | }
|
---|
81 | else {
|
---|
82 | _epoSpec = true;
|
---|
83 | }
|
---|
84 | _sigma0 = OPT->sigmaIono();
|
---|
85 | _noise = OPT->noiseIono();
|
---|
86 | break;
|
---|
87 | case bias:
|
---|
88 | _epoSpec = true;
|
---|
89 | _sigma0 = 10.0;
|
---|
90 | break;
|
---|
91 | }
|
---|
92 | }
|
---|
93 |
|
---|
94 | // Destructor
|
---|
95 | ////////////////////////////////////////////////////////////////////////////
|
---|
96 | t_param::~t_param() {
|
---|
97 | delete _ambInfo;
|
---|
98 | }
|
---|
99 |
|
---|
100 | //
|
---|
101 | ////////////////////////////////////////////////////////////////////////////
|
---|
102 | double t_param::partial(const bncTime& epoTime, const t_satObs* obs,
|
---|
103 | const t_lc::type& tLC) const {
|
---|
104 |
|
---|
105 | // Special Case - Melbourne-Wuebbena
|
---|
106 | // ---------------------------------
|
---|
107 | if (tLC == t_lc::MW && _type != amb && _type != bias) {
|
---|
108 | return 0.0;
|
---|
109 | }
|
---|
110 |
|
---|
111 | const t_station* sta = PPP_CLIENT->staRover();
|
---|
112 | ColumnVector rhoV = sta->xyzApr() - obs->xc().Rows(1,3);
|
---|
113 |
|
---|
114 | switch (_type) {
|
---|
115 | case crdX:
|
---|
116 | return (sta->xyzApr()[0] - obs->xc()[0]) / rhoV.norm_Frobenius();
|
---|
117 | case crdY:
|
---|
118 | return (sta->xyzApr()[1] - obs->xc()[1]) / rhoV.norm_Frobenius();
|
---|
119 | case crdZ:
|
---|
120 | return (sta->xyzApr()[2] - obs->xc()[2]) / rhoV.norm_Frobenius();
|
---|
121 | case clkR:
|
---|
122 | return 1.0;
|
---|
123 | case offGG:
|
---|
124 | return (obs->prn().system() == 'R') ? 1.0 : 0.0;
|
---|
125 | case amb:
|
---|
126 | if (obs->prn() == _prn) {
|
---|
127 | if (tLC == _tLC) {
|
---|
128 | return (obs->lambda(tLC));
|
---|
129 | }
|
---|
130 | else if (tLC == t_lc::lIF && _tLC == t_lc::MW) {
|
---|
131 | return obs->lambda(t_lc::lIF) * obs->lambda(t_lc::MW) / obs->lambda(t_lc::l2);
|
---|
132 | }
|
---|
133 | else {
|
---|
134 | ColumnVector coeff(4);
|
---|
135 | obs->lc(tLC, 0.0, 0.0, 0.0, 0.0, &coeff);
|
---|
136 | if (_tLC == t_lc::l1) {
|
---|
137 | return obs->lambda(t_lc::l1) * coeff(1);
|
---|
138 | }
|
---|
139 | else if (_tLC == t_lc::l2) {
|
---|
140 | return obs->lambda(t_lc::l2) * coeff(2);
|
---|
141 | }
|
---|
142 | }
|
---|
143 | }
|
---|
144 | return 0.0;
|
---|
145 | case trp:
|
---|
146 | return t_tropo::mf(OPT->tropoMF(), epoTime, sta->ellApr()[0],
|
---|
147 | sta->ellApr()[1], sta->ellApr()[2], obs->eleSat());
|
---|
148 | case iono:
|
---|
149 | if (obs->prn() == _prn) {
|
---|
150 | return t_iono::dLCdI(tLC, obs->prn().system(), obs->channel());
|
---|
151 | }
|
---|
152 | else {
|
---|
153 | return 0.0;
|
---|
154 | }
|
---|
155 | case bias:
|
---|
156 | if (tLC == _tLC && obs->prn().system() == _prn.system()) {
|
---|
157 | return 1.0;
|
---|
158 | }
|
---|
159 | else {
|
---|
160 | return 0.0;
|
---|
161 | }
|
---|
162 | }
|
---|
163 |
|
---|
164 | return 0.0;
|
---|
165 | }
|
---|
166 |
|
---|
167 | //
|
---|
168 | ////////////////////////////////////////////////////////////////////////////
|
---|
169 | string t_param::toString() const {
|
---|
170 | stringstream ss;
|
---|
171 | switch (_type) {
|
---|
172 | case crdX:
|
---|
173 | ss << "CRD_X";
|
---|
174 | break;
|
---|
175 | case crdY:
|
---|
176 | ss << "CRD_Y";
|
---|
177 | break;
|
---|
178 | case crdZ:
|
---|
179 | ss << "CRD_Z";
|
---|
180 | break;
|
---|
181 | case clkR:
|
---|
182 | ss << "CLK ";
|
---|
183 | break;
|
---|
184 | case amb:
|
---|
185 | ss << "AMB " << left << setw(3) << t_lc::toString(_tLC) << right << ' ' << _prn.toString();
|
---|
186 | break;
|
---|
187 | case offGG:
|
---|
188 | ss << "OGG ";
|
---|
189 | break;
|
---|
190 | case trp:
|
---|
191 | ss << "TRP ";
|
---|
192 | break;
|
---|
193 | case iono:
|
---|
194 | ss << "IONO " << _prn.toString();
|
---|
195 | break;
|
---|
196 | case bias:
|
---|
197 | ss << "BIAS " << _prn.system() << ' ' << left << setw(3) << t_lc::toString(_tLC);
|
---|
198 | break;
|
---|
199 | }
|
---|
200 | return ss.str();
|
---|
201 | }
|
---|
202 |
|
---|
203 | // Constructor
|
---|
204 | ////////////////////////////////////////////////////////////////////////////
|
---|
205 | t_parlist::t_parlist() {
|
---|
206 | }
|
---|
207 |
|
---|
208 | // Destructor
|
---|
209 | ////////////////////////////////////////////////////////////////////////////
|
---|
210 | t_parlist::~t_parlist() {
|
---|
211 | for (unsigned ii = 0; ii < _params.size(); ii++) {
|
---|
212 | delete _params[ii];
|
---|
213 | }
|
---|
214 | }
|
---|
215 |
|
---|
216 | //
|
---|
217 | ////////////////////////////////////////////////////////////////////////////
|
---|
218 | t_irc t_parlist::set(const bncTime& epoTime, const vector<t_lc::type>& ambLCs,
|
---|
219 | const vector<t_satObs*>& obsVector) {
|
---|
220 |
|
---|
221 | // Remove some Parameters
|
---|
222 | // ----------------------
|
---|
223 | vector<t_param*>::iterator it = _params.begin();
|
---|
224 | while (it != _params.end()) {
|
---|
225 | t_param* par = *it;
|
---|
226 |
|
---|
227 | bool remove = false;
|
---|
228 |
|
---|
229 | if (par->epoSpec()) {
|
---|
230 | remove = true;
|
---|
231 | }
|
---|
232 |
|
---|
233 | else if (par->type() == t_param::amb) {
|
---|
234 | if (par->lastObsTime().valid() && (epoTime - par->lastObsTime() > 120.0)) {
|
---|
235 | remove = true;
|
---|
236 | }
|
---|
237 | }
|
---|
238 |
|
---|
239 | else if (par->type() == t_param::amb || par->type() == t_param::iono) {
|
---|
240 | if (par->lastObsTime().valid() && (epoTime - par->lastObsTime() > 3600.0)) {
|
---|
241 | remove = true;
|
---|
242 | }
|
---|
243 | }
|
---|
244 |
|
---|
245 | if (remove) {
|
---|
246 | delete par;
|
---|
247 | it = _params.erase(it);
|
---|
248 | }
|
---|
249 | else {
|
---|
250 | ++it;
|
---|
251 | }
|
---|
252 | }
|
---|
253 |
|
---|
254 | // Check whether parameters have observations
|
---|
255 | // ------------------------------------------
|
---|
256 | for (unsigned ii = 0; ii < _params.size(); ii++) {
|
---|
257 | t_param* par = _params[ii];
|
---|
258 | if (par->prn() == 0) {
|
---|
259 | par->setLastObsTime(epoTime);
|
---|
260 | if (par->firstObsTime().undef()) {
|
---|
261 | par->setFirstObsTime(epoTime);
|
---|
262 | }
|
---|
263 | }
|
---|
264 | else {
|
---|
265 | for (unsigned jj = 0; jj < obsVector.size(); jj++) {
|
---|
266 | const t_satObs* satObs = obsVector[jj];
|
---|
267 | if (satObs->prn() == par->prn()) {
|
---|
268 | par->setLastObsTime(epoTime);
|
---|
269 | if (par->firstObsTime().undef()) {
|
---|
270 | par->setFirstObsTime(epoTime);
|
---|
271 | }
|
---|
272 | break;
|
---|
273 | }
|
---|
274 | }
|
---|
275 | }
|
---|
276 | }
|
---|
277 |
|
---|
278 | // Required Set of Parameters
|
---|
279 | // --------------------------
|
---|
280 | vector<t_param*> required;
|
---|
281 |
|
---|
282 | // Coordinates
|
---|
283 | // -----------
|
---|
284 | required.push_back(new t_param(t_param::crdX, t_prn(), t_lc::dummy));
|
---|
285 | required.push_back(new t_param(t_param::crdY, t_prn(), t_lc::dummy));
|
---|
286 | required.push_back(new t_param(t_param::crdZ, t_prn(), t_lc::dummy));
|
---|
287 |
|
---|
288 | // Receiver Clock
|
---|
289 | // --------------
|
---|
290 | required.push_back(new t_param(t_param::clkR, t_prn(), t_lc::dummy));
|
---|
291 |
|
---|
292 | // GPS-Glonass Clock Offset
|
---|
293 | // ------------------------
|
---|
294 | if (OPT->useGlonass()) {
|
---|
295 | required.push_back(new t_param(t_param::offGG, t_prn(), t_lc::dummy));
|
---|
296 | }
|
---|
297 |
|
---|
298 | // Receiver Biases
|
---|
299 | // ---------------
|
---|
300 | for (unsigned ii = 0; ii < OPT->estBias().size(); ii++) {
|
---|
301 | const t_options::t_optBias& optBias = OPT->estBias()[ii];
|
---|
302 | required.push_back(new t_param(t_param::bias, t_prn(optBias._system, 1), optBias._tLC));
|
---|
303 | }
|
---|
304 |
|
---|
305 | // Troposphere
|
---|
306 | // -----------
|
---|
307 | if (OPT->estTropo()) {
|
---|
308 | required.push_back(new t_param(t_param::trp, t_prn(), t_lc::dummy));
|
---|
309 | }
|
---|
310 |
|
---|
311 | // Ambiguities
|
---|
312 | // -----------
|
---|
313 | for (unsigned ii = 0; ii < ambLCs.size(); ii++) {
|
---|
314 | const t_lc::type& tLC = ambLCs[ii];
|
---|
315 | for (unsigned jj = 0; jj < obsVector.size(); jj++) {
|
---|
316 | const t_satObs* satObs = obsVector[jj];
|
---|
317 | required.push_back(new t_param(t_param::amb, satObs->prn(), tLC, &obsVector));
|
---|
318 | }
|
---|
319 | }
|
---|
320 |
|
---|
321 | // Stochastic Ionosphere
|
---|
322 | //----------------------
|
---|
323 | if (OPT->estIono()) {
|
---|
324 | for (unsigned jj = 0; jj < obsVector.size(); jj++) {
|
---|
325 | const t_satObs* satObs = obsVector[jj];
|
---|
326 | required.push_back(new t_param(t_param::iono, satObs->prn(), t_lc::dummy));
|
---|
327 | }
|
---|
328 | }
|
---|
329 |
|
---|
330 | // Check if all required parameters are present
|
---|
331 | // --------------------------------------------
|
---|
332 | for (unsigned ii = 0; ii < required.size(); ii++) {
|
---|
333 | t_param* parReq = required[ii];
|
---|
334 |
|
---|
335 | bool found = false;
|
---|
336 | for (unsigned jj = 0; jj < _params.size(); jj++) {
|
---|
337 | t_param* parOld = _params[jj];
|
---|
338 | if (parOld->isEqual(parReq)) {
|
---|
339 | found = true;
|
---|
340 | break;
|
---|
341 | }
|
---|
342 | }
|
---|
343 | if (found) {
|
---|
344 | delete parReq;
|
---|
345 | }
|
---|
346 | else {
|
---|
347 | _params.push_back(parReq);
|
---|
348 | }
|
---|
349 | }
|
---|
350 |
|
---|
351 | // Set Parameter Indices
|
---|
352 | // ---------------------
|
---|
353 | sort(_params.begin(), _params.end(), t_param::sortFunction);
|
---|
354 |
|
---|
355 | for (unsigned ii = 0; ii < _params.size(); ii++) {
|
---|
356 | t_param* par = _params[ii];
|
---|
357 | par->setIndex(ii);
|
---|
358 | for (unsigned jj = 0; jj < obsVector.size(); jj++) {
|
---|
359 | const t_satObs* satObs = obsVector[jj];
|
---|
360 | if (satObs->prn() == par->prn()) {
|
---|
361 | par->setAmbEleSat(satObs->eleSat());
|
---|
362 | par->stepAmbNumEpo();
|
---|
363 | }
|
---|
364 | }
|
---|
365 | }
|
---|
366 |
|
---|
367 | return t_irc::success;
|
---|
368 | }
|
---|
369 |
|
---|
370 | //
|
---|
371 | ////////////////////////////////////////////////////////////////////////////
|
---|
372 | void t_parlist::printResult(const bncTime& epoTime, const SymmetricMatrix& QQ,
|
---|
373 | const ColumnVector& xx, double ambFixRate) const {
|
---|
374 |
|
---|
375 | string epoTimeStr = string(epoTime);
|
---|
376 |
|
---|
377 | LOG << endl;
|
---|
378 |
|
---|
379 | t_param* parX = 0;
|
---|
380 | t_param* parY = 0;
|
---|
381 | t_param* parZ = 0;
|
---|
382 | for (unsigned ii = 0; ii < _params.size(); ii++) {
|
---|
383 | t_param* par = _params[ii];
|
---|
384 | if (par->type() == t_param::crdX) {
|
---|
385 | parX = par;
|
---|
386 | }
|
---|
387 | else if (par->type() == t_param::crdY) {
|
---|
388 | parY = par;
|
---|
389 | }
|
---|
390 | else if (par->type() == t_param::crdZ) {
|
---|
391 | parZ = par;
|
---|
392 | }
|
---|
393 | else {
|
---|
394 | int ind = par->indexNew();
|
---|
395 | LOG << epoTimeStr << ' ' << par->toString() << ' '
|
---|
396 | << setw(10) << setprecision(4) << par->x0() << ' '
|
---|
397 | << showpos << setw(10) << setprecision(4) << xx[ind] << noshowpos << " +- "
|
---|
398 | << setw(8) << setprecision(4) << sqrt(QQ[ind][ind]);
|
---|
399 | if (par->type() == t_param::amb) {
|
---|
400 | LOG << " el = " << setw(6) << setprecision(2) << par->ambEleSat() * t_genConst::rho_deg
|
---|
401 | << " epo = " << setw(4) << par->ambNumEpo();
|
---|
402 | }
|
---|
403 | LOG << endl;
|
---|
404 | }
|
---|
405 | }
|
---|
406 |
|
---|
407 | if (parX && parY && parZ) {
|
---|
408 | const t_station* sta = PPP_CLIENT->staRover();
|
---|
409 |
|
---|
410 | ColumnVector xyz(3);
|
---|
411 | xyz[0] = xx[parX->indexNew()];
|
---|
412 | xyz[1] = xx[parY->indexNew()];
|
---|
413 | xyz[2] = xx[parZ->indexNew()];
|
---|
414 |
|
---|
415 | ColumnVector neu(3);
|
---|
416 | xyz2neu(sta->ellApr().data(), xyz.data(), neu.data());
|
---|
417 |
|
---|
418 | SymmetricMatrix QQxyz = QQ.SymSubMatrix(1,3);
|
---|
419 |
|
---|
420 | SymmetricMatrix QQneu(3);
|
---|
421 | covariXYZ_NEU(QQxyz, sta->ellApr().data(), QQneu);
|
---|
422 |
|
---|
423 | LOG << epoTimeStr
|
---|
424 | << " X = " << setprecision(4) << sta->xyzApr()[0] + xyz[0] << " +- "
|
---|
425 | << setprecision(4) << sqrt(QQxyz[0][0])
|
---|
426 |
|
---|
427 | << " Y = " << setprecision(4) << sta->xyzApr()[1] + xyz[1] << " +- "
|
---|
428 | << setprecision(4) << sqrt(QQxyz[1][1])
|
---|
429 |
|
---|
430 | << " Z = " << setprecision(4) << sta->xyzApr()[2] + xyz[2] << " +- "
|
---|
431 | << setprecision(4) << sqrt(QQxyz[2][2])
|
---|
432 |
|
---|
433 | << " dN = " << setprecision(4) << neu[0] << " +- "
|
---|
434 | << setprecision(4) << sqrt(QQneu[0][0])
|
---|
435 |
|
---|
436 | << " dE = " << setprecision(4) << neu[1] << " +- "
|
---|
437 | << setprecision(4) << sqrt(QQneu[1][1])
|
---|
438 |
|
---|
439 | << " dU = " << setprecision(4) << neu[2] << " +- "
|
---|
440 | << setprecision(4) << sqrt(QQneu[2][2]);
|
---|
441 | if (ambFixRate > 0.0) {
|
---|
442 | LOG << " fix ";
|
---|
443 | }
|
---|
444 | else {
|
---|
445 | LOG << " flt ";
|
---|
446 | }
|
---|
447 | LOG << int(100*ambFixRate) << " %\n";
|
---|
448 | }
|
---|
449 | }
|
---|
450 |
|
---|