source: ntrip/trunk/BNC/src/PPP/parlist.cpp@ 5743

Last change on this file since 5743 was 5743, checked in by mervart, 10 years ago
File size: 11.6 KB
Line 
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
15using namespace BNC;
16using namespace std;
17
18// Constructor
19////////////////////////////////////////////////////////////////////////////
20t_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////////////////////////////////////////////////////////////////////////////
96t_param::~t_param() {
97 delete _ambInfo;
98}
99
100//
101////////////////////////////////////////////////////////////////////////////
102double 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////////////////////////////////////////////////////////////////////////////
169string 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////////////////////////////////////////////////////////////////////////////
205t_parlist::t_parlist() {
206}
207
208// Destructor
209////////////////////////////////////////////////////////////////////////////
210t_parlist::~t_parlist() {
211 for (unsigned ii = 0; ii < _params.size(); ii++) {
212 delete _params[ii];
213 }
214}
215
216//
217////////////////////////////////////////////////////////////////////////////
218t_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////////////////////////////////////////////////////////////////////////////
372void 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
Note: See TracBrowser for help on using the repository browser.