source: ntrip/trunk/BNC/src/PPP/pppParlist.cpp@ 5809

Last change on this file since 5809 was 5809, checked in by mervart, 10 years ago
File size: 10.8 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
10#include "station.h"
11#include "bncutils.h"
12#include "bncconst.h"
13#include "pppClient.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->_sigCrd[0];
36 break;
37 case crdY:
38 _epoSpec = true;
39 _sigma0 = OPT->_sigCrd[1];
40 break;
41 case crdZ:
42 _epoSpec = true;
43 _sigma0 = OPT->_sigCrd[2];
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 = floor((obs->obsValue(tLC) - offGG - obs->cmpValue(tLC)) / obs->lambda(tLC) + 0.5);
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->_sigTropo;
75 _noise = OPT->_noiseTropo;
76 break;
77 case bias:
78 _epoSpec = true;
79 _sigma0 = 10.0;
80 break;
81 }
82}
83
84// Destructor
85////////////////////////////////////////////////////////////////////////////
86t_param::~t_param() {
87 delete _ambInfo;
88}
89
90//
91////////////////////////////////////////////////////////////////////////////
92double t_param::partial(const bncTime& /* epoTime */, const t_satObs* obs,
93 const t_lc::type& tLC) const {
94
95 // Special Case - Melbourne-Wuebbena
96 // ---------------------------------
97 if (tLC == t_lc::MW && _type != amb && _type != bias) {
98 return 0.0;
99 }
100
101 const t_station* sta = PPP_CLIENT->staRover();
102 ColumnVector rhoV = sta->xyzApr() - obs->xc().Rows(1,3);
103
104 switch (_type) {
105 case crdX:
106 return (sta->xyzApr()[0] - obs->xc()[0]) / rhoV.norm_Frobenius();
107 case crdY:
108 return (sta->xyzApr()[1] - obs->xc()[1]) / rhoV.norm_Frobenius();
109 case crdZ:
110 return (sta->xyzApr()[2] - obs->xc()[2]) / rhoV.norm_Frobenius();
111 case clkR:
112 return 1.0;
113 case offGG:
114 return (obs->prn().system() == 'R') ? 1.0 : 0.0;
115 case amb:
116 if (obs->prn() == _prn) {
117 if (tLC == _tLC) {
118 return (obs->lambda(tLC));
119 }
120 else if (tLC == t_lc::lIF && _tLC == t_lc::MW) {
121 return obs->lambda(t_lc::lIF) * obs->lambda(t_lc::MW) / obs->lambda(t_lc::l2);
122 }
123 else {
124 ColumnVector coeff(4);
125 obs->lc(tLC, 0.0, 0.0, 0.0, 0.0, &coeff);
126 if (_tLC == t_lc::l1) {
127 return obs->lambda(t_lc::l1) * coeff(1);
128 }
129 else if (_tLC == t_lc::l2) {
130 return obs->lambda(t_lc::l2) * coeff(2);
131 }
132 }
133 }
134 return 0.0;
135 case trp:
136 return 1.0 / sin(obs->eleSat());
137 case bias:
138 if (tLC == _tLC && obs->prn().system() == _prn.system()) {
139 return 1.0;
140 }
141 else {
142 return 0.0;
143 }
144 }
145
146 return 0.0;
147}
148
149//
150////////////////////////////////////////////////////////////////////////////
151string t_param::toString() const {
152 stringstream ss;
153 switch (_type) {
154 case crdX:
155 ss << "CRD_X";
156 break;
157 case crdY:
158 ss << "CRD_Y";
159 break;
160 case crdZ:
161 ss << "CRD_Z";
162 break;
163 case clkR:
164 ss << "CLK ";
165 break;
166 case amb:
167 ss << "AMB " << left << setw(3) << t_lc::toString(_tLC) << right << ' ' << _prn.toString();
168 break;
169 case offGG:
170 ss << "OGG ";
171 break;
172 case trp:
173 ss << "TRP ";
174 break;
175 case bias:
176 ss << "BIAS " << _prn.system() << ' ' << left << setw(3) << t_lc::toString(_tLC);
177 break;
178 }
179 return ss.str();
180}
181
182// Constructor
183////////////////////////////////////////////////////////////////////////////
184t_parlist::t_parlist() {
185}
186
187// Destructor
188////////////////////////////////////////////////////////////////////////////
189t_parlist::~t_parlist() {
190 for (unsigned ii = 0; ii < _params.size(); ii++) {
191 delete _params[ii];
192 }
193}
194
195//
196////////////////////////////////////////////////////////////////////////////
197t_irc t_parlist::set(const bncTime& epoTime, const vector<t_lc::type>& ambLCs,
198 const vector<t_satObs*>& obsVector) {
199
200 // Remove some Parameters
201 // ----------------------
202 vector<t_param*>::iterator it = _params.begin();
203 while (it != _params.end()) {
204 t_param* par = *it;
205
206 bool remove = false;
207
208 if (par->epoSpec()) {
209 remove = true;
210 }
211
212 else if (par->type() == t_param::amb) {
213 if (par->lastObsTime().valid() && (epoTime - par->lastObsTime() > 120.0)) {
214 remove = true;
215 }
216 }
217
218 else if (par->type() == t_param::amb) {
219 if (par->lastObsTime().valid() && (epoTime - par->lastObsTime() > 3600.0)) {
220 remove = true;
221 }
222 }
223
224 if (remove) {
225 delete par;
226 it = _params.erase(it);
227 }
228 else {
229 ++it;
230 }
231 }
232
233 // Check whether parameters have observations
234 // ------------------------------------------
235 for (unsigned ii = 0; ii < _params.size(); ii++) {
236 t_param* par = _params[ii];
237 if (par->prn() == 0) {
238 par->setLastObsTime(epoTime);
239 if (par->firstObsTime().undef()) {
240 par->setFirstObsTime(epoTime);
241 }
242 }
243 else {
244 for (unsigned jj = 0; jj < obsVector.size(); jj++) {
245 const t_satObs* satObs = obsVector[jj];
246 if (satObs->prn() == par->prn()) {
247 par->setLastObsTime(epoTime);
248 if (par->firstObsTime().undef()) {
249 par->setFirstObsTime(epoTime);
250 }
251 break;
252 }
253 }
254 }
255 }
256
257 // Required Set of Parameters
258 // --------------------------
259 vector<t_param*> required;
260
261 // Coordinates
262 // -----------
263 required.push_back(new t_param(t_param::crdX, t_prn(), t_lc::dummy));
264 required.push_back(new t_param(t_param::crdY, t_prn(), t_lc::dummy));
265 required.push_back(new t_param(t_param::crdZ, t_prn(), t_lc::dummy));
266
267 // Receiver Clock
268 // --------------
269 required.push_back(new t_param(t_param::clkR, t_prn(), t_lc::dummy));
270
271 // GPS-Glonass Clock Offset
272 // ------------------------
273 if (OPT->useGlonass()) {
274 required.push_back(new t_param(t_param::offGG, t_prn(), t_lc::dummy));
275 }
276
277 // Receiver Biases
278 // ---------------
279 for (unsigned ii = 0; ii < OPT->_estBias.size(); ii++) {
280 const t_options::t_optBias& optBias = OPT->_estBias[ii];
281 required.push_back(new t_param(t_param::bias, t_prn(optBias._system, 1), optBias._tLC));
282 }
283
284 // Troposphere
285 // -----------
286 if (OPT->estTropo()) {
287 required.push_back(new t_param(t_param::trp, t_prn(), t_lc::dummy));
288 }
289
290 // Ambiguities
291 // -----------
292 for (unsigned ii = 0; ii < ambLCs.size(); ii++) {
293 const t_lc::type& tLC = ambLCs[ii];
294 for (unsigned jj = 0; jj < obsVector.size(); jj++) {
295 const t_satObs* satObs = obsVector[jj];
296 required.push_back(new t_param(t_param::amb, satObs->prn(), tLC, &obsVector));
297 }
298 }
299
300 // Check if all required parameters are present
301 // --------------------------------------------
302 for (unsigned ii = 0; ii < required.size(); ii++) {
303 t_param* parReq = required[ii];
304
305 bool found = false;
306 for (unsigned jj = 0; jj < _params.size(); jj++) {
307 t_param* parOld = _params[jj];
308 if (parOld->isEqual(parReq)) {
309 found = true;
310 break;
311 }
312 }
313 if (found) {
314 delete parReq;
315 }
316 else {
317 _params.push_back(parReq);
318 }
319 }
320
321 // Set Parameter Indices
322 // ---------------------
323 sort(_params.begin(), _params.end(), t_param::sortFunction);
324
325 for (unsigned ii = 0; ii < _params.size(); ii++) {
326 t_param* par = _params[ii];
327 par->setIndex(ii);
328 for (unsigned jj = 0; jj < obsVector.size(); jj++) {
329 const t_satObs* satObs = obsVector[jj];
330 if (satObs->prn() == par->prn()) {
331 par->setAmbEleSat(satObs->eleSat());
332 par->stepAmbNumEpo();
333 }
334 }
335 }
336
337 return success;
338}
339
340//
341////////////////////////////////////////////////////////////////////////////
342void t_parlist::printResult(const bncTime& epoTime, const SymmetricMatrix& QQ,
343 const ColumnVector& xx, double ambFixRate) const {
344
345 string epoTimeStr = string(epoTime);
346
347 LOG << endl;
348
349 t_param* parX = 0;
350 t_param* parY = 0;
351 t_param* parZ = 0;
352 for (unsigned ii = 0; ii < _params.size(); ii++) {
353 t_param* par = _params[ii];
354 if (par->type() == t_param::crdX) {
355 parX = par;
356 }
357 else if (par->type() == t_param::crdY) {
358 parY = par;
359 }
360 else if (par->type() == t_param::crdZ) {
361 parZ = par;
362 }
363 else {
364 int ind = par->indexNew();
365 LOG << epoTimeStr << ' ' << par->toString() << ' '
366 << setw(10) << setprecision(4) << par->x0() << ' '
367 << showpos << setw(10) << setprecision(4) << xx[ind] << noshowpos << " +- "
368 << setw(8) << setprecision(4) << sqrt(QQ[ind][ind]);
369 if (par->type() == t_param::amb) {
370 LOG << " el = " << setw(6) << setprecision(2) << par->ambEleSat() * 180.0 / M_PI
371 << " epo = " << setw(4) << par->ambNumEpo();
372 }
373 LOG << endl;
374 }
375 }
376
377 if (parX && parY && parZ) {
378 const t_station* sta = PPP_CLIENT->staRover();
379
380 ColumnVector xyz(3);
381 xyz[0] = xx[parX->indexNew()];
382 xyz[1] = xx[parY->indexNew()];
383 xyz[2] = xx[parZ->indexNew()];
384
385 ColumnVector neu(3);
386 xyz2neu(sta->ellApr().data(), xyz.data(), neu.data());
387
388 SymmetricMatrix QQxyz = QQ.SymSubMatrix(1,3);
389
390 SymmetricMatrix QQneu(3);
391 covariXYZ_NEU(QQxyz, sta->ellApr().data(), QQneu);
392
393 LOG << epoTimeStr
394 << " X = " << setprecision(4) << sta->xyzApr()[0] + xyz[0] << " +- "
395 << setprecision(4) << sqrt(QQxyz[0][0])
396
397 << " Y = " << setprecision(4) << sta->xyzApr()[1] + xyz[1] << " +- "
398 << setprecision(4) << sqrt(QQxyz[1][1])
399
400 << " Z = " << setprecision(4) << sta->xyzApr()[2] + xyz[2] << " +- "
401 << setprecision(4) << sqrt(QQxyz[2][2])
402
403 << " dN = " << setprecision(4) << neu[0] << " +- "
404 << setprecision(4) << sqrt(QQneu[0][0])
405
406 << " dE = " << setprecision(4) << neu[1] << " +- "
407 << setprecision(4) << sqrt(QQneu[1][1])
408
409 << " dU = " << setprecision(4) << neu[2] << " +- "
410 << setprecision(4) << sqrt(QQneu[2][2]);
411 if (ambFixRate > 0.0) {
412 LOG << " fix ";
413 }
414 else {
415 LOG << " flt ";
416 }
417 LOG << int(100*ambFixRate) << " %\n";
418 }
419}
420
Note: See TracBrowser for help on using the repository browser.