00001 #pragma ident "$Id: ModeledReferencePR.cpp 1379 2008-08-29 17:04:02Z architest $"
00002
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #include "ModeledReferencePR.hpp"
00033
00034 using namespace std;
00035 namespace gpstk
00036 {
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056 ModeledReferencePR::ModeledReferencePR( const Position& RxCoordinates,
00057 IonoModelStore& dIonoModel,
00058 TropModel& dTropoModel,
00059 XvtStore<SatID>& dEphemeris,
00060 const TypeID& dObservable,
00061 bool usetgd )
00062 {
00063 init();
00064 setInitialRxPosition(RxCoordinates);
00065 setDefaultIonoModel(dIonoModel);
00066 setDefaultTropoModel(dTropoModel);
00067 setDefaultObservable(dObservable);
00068 setDefaultEphemeris(dEphemeris);
00069 useTGD = usetgd;
00070 }
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092 ModeledReferencePR::ModeledReferencePR( const Position& RxCoordinates,
00093 IonoModelStore& dIonoModel,
00094 XvtStore<SatID>& dEphemeris,
00095 const TypeID& dObservable,
00096 bool usetgd )
00097 {
00098 init();
00099 setInitialRxPosition(RxCoordinates);
00100 setDefaultIonoModel(dIonoModel);
00101 setDefaultObservable(dObservable);
00102 setDefaultEphemeris(dEphemeris);
00103 useTGD = usetgd;
00104 pDefaultTropoModel = NULL;
00105 }
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127 ModeledReferencePR::ModeledReferencePR( const Position& RxCoordinates,
00128 TropModel& dTropoModel,
00129 XvtStore<SatID>& dEphemeris,
00130 const TypeID& dObservable,
00131 bool usetgd )
00132 {
00133 init();
00134 setInitialRxPosition(RxCoordinates);
00135 setDefaultTropoModel(dTropoModel);
00136 setDefaultObservable(dObservable);
00137 setDefaultEphemeris(dEphemeris);
00138 useTGD = usetgd;
00139 pDefaultIonoModel = NULL;
00140 }
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161 ModeledReferencePR::ModeledReferencePR( const Position& RxCoordinates,
00162 XvtStore<SatID>& dEphemeris,
00163 const TypeID& dObservable,
00164 bool usetgd )
00165 {
00166 init();
00167 setInitialRxPosition(RxCoordinates);
00168 setDefaultObservable(dObservable);
00169 setDefaultEphemeris(dEphemeris);
00170 useTGD = usetgd;
00171 pDefaultIonoModel = NULL;
00172 pDefaultTropoModel = NULL;
00173 }
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193 int ModeledReferencePR::Compute( const DayTime& Tr,
00194 Vector<SatID>& Satellite,
00195 Vector<double>& Pseudorange,
00196 const XvtStore<SatID>& Eph,
00197 const Vector<double>& extraBiases,
00198 TropModel *pTropModel,
00199 IonoModelStore *pIonoModel )
00200 throw(Exception)
00201 {
00202 try
00203 {
00204
00205 int N = Satellite.size();
00206 if(N <= 0) return 0;
00207
00208 int i;
00209
00210 int eN = int(extraBiases.size()) - 1;
00211 int validSats(0);
00212
00213
00214 vector<double> vPR;
00215 vector<double> vGeometricRho;
00216 vector<double> vClock;
00217 vector<double> vTGD;
00218 vector<double> vRel;
00219 vector<double> vTrop;
00220 vector<double> vIono;
00221 vector<double> vObservedPR;
00222 vector<double> vModeledPR;
00223 vector<double> vPrefit;
00224 vector<double> vElevation;
00225 vector<double> vAzimuth;
00226 vector<Xvt> vXvt;
00227 vector<DayTime> vTxTime;
00228 vector<SatID> vAvailableSV;
00229 vector<SatID> vRejectedSV;
00230 vector<Triple> vCosines;
00231 vector<Triple>::iterator iter;
00232
00233 CorrectedEphemerisRange cerange;
00234 validData = false;
00235
00236
00237 rejectedSV.resize(0);
00238 availableSV.resize(0);
00239 geometricRho.resize(0);
00240 svClockBiases.resize(0);
00241 svXvt.resize(0);
00242 svTxTime.resize(0);
00243 svTGD.resize(0);
00244 svRelativity.resize(0);
00245 ionoCorrections.resize(0);
00246 tropoCorrections.resize(0);
00247 observedPseudoranges.resize(0);
00248 modeledPseudoranges.resize(0);
00249 prefitResiduals.resize(0);
00250 elevationSV.resize(0);
00251 azimuthSV.resize(0);
00252 geoMatrix.resize(0, 0);
00253
00254 for (i=0; i<N; i++)
00255 {
00256
00257 if(Satellite[i].id <= 0)
00258 {
00259
00260 SatID tempSat( std::abs(Satellite[i].id),
00261 Satellite[i].system);
00262 vRejectedSV.push_back(tempSat);
00263 continue;
00264 }
00265
00266 try
00267 {
00268
00269 double tempPR(0.0);
00270 double tempTGD(0.0);
00271 double tempTrop(0.0);
00272 double tempIono(0.0);
00273 double tempModeledPR(0.0);
00274 double tempPrefit(0.0);
00275
00276 try
00277 {
00278
00279 tempPR = cerange.ComputeAtTransmitTime( Tr,
00280 Pseudorange[i],
00281 rxPos,
00282 Satellite[i],
00283 Eph );
00284 }
00285 catch(InvalidRequest& e)
00286 {
00287
00288
00289 vRejectedSV.push_back(Satellite[i]);
00290 continue;
00291 };
00292
00293
00294 if(rxPos.elevationGeodetic(cerange.svPosVel) < (*this).minElev)
00295 {
00296
00297
00298 vRejectedSV.push_back(Satellite[i]);
00299 continue;
00300 };
00301
00302
00303 if(pTropModel)
00304 {
00305 tempTrop = getTropoCorrections( pTropModel,
00306 cerange.elevationGeodetic );
00307 };
00308
00309
00310 if(pIonoModel)
00311 {
00312
00313 Geodetic rxGeo( rxPos.getGeodeticLatitude(),
00314 rxPos.getLongitude(),
00315 rxPos.getAltitude() );
00316
00317 tempIono = getIonoCorrections( pIonoModel,
00318 Tr,
00319 rxGeo,
00320 cerange.elevationGeodetic,
00321 cerange.azimuthGeodetic );
00322 };
00323
00324 tempModeledPR = tempPR + tempTrop + tempIono;
00325
00326
00327
00328 if (i <= eN )
00329 {
00330 tempModeledPR += extraBiases(i);
00331 };
00332
00333
00334
00335 if(useTGD)
00336 {
00337 tempTGD = getTGDCorrections(Tr, Eph, Satellite[i]);
00338 tempModeledPR += tempTGD;
00339 }
00340
00341 tempPrefit = Pseudorange[i] - tempModeledPR;
00342
00343
00344 vGeometricRho.push_back(cerange.rawrange);
00345 vClock.push_back(cerange.svclkbias);
00346 vXvt.push_back(cerange.svPosVel);
00347 vTxTime.push_back(cerange.transmit);
00348 vTGD.push_back(tempTGD);
00349
00350 vRel.push_back(-cerange.relativity);
00351 vIono.push_back(tempIono);
00352 vTrop.push_back(tempTrop);
00353 vObservedPR.push_back(Pseudorange[i]);
00354 vModeledPR.push_back(tempModeledPR);
00355 vPrefit.push_back(tempPrefit);
00356 vElevation.push_back(cerange.elevationGeodetic);
00357 vAzimuth.push_back(cerange.azimuthGeodetic);
00358 vAvailableSV.push_back(Satellite[i]);
00359 vCosines.push_back(cerange.cosines);
00360
00361
00362 validSats += 1;
00363
00364 }
00365 catch(InvalidRequest& e)
00366 {
00367
00368
00369 vRejectedSV.push_back(Satellite[i]);
00370 continue;
00371 }
00372 catch(...)
00373 {
00374 Exception unknownEx( "An unknown exception has happened in \
00375 ModeledReferencePR object." );
00376 GPSTK_THROW(unknownEx);
00377 }
00378
00379 }
00380
00381
00382 rejectedSV = vRejectedSV;
00383 availableSV = vAvailableSV;
00384 geometricRho = vGeometricRho;
00385 svClockBiases = vClock;
00386 svXvt = vXvt;
00387 svTxTime = vTxTime;
00388 svTGD = vTGD;
00389 svRelativity = vRel;
00390 ionoCorrections = vIono;
00391 tropoCorrections = vTrop;
00392 observedPseudoranges = vObservedPR;
00393 modeledPseudoranges = vModeledPR;
00394 prefitResiduals = vPrefit;
00395 elevationSV = vElevation;
00396 azimuthSV = vAzimuth;
00397
00398
00399
00400 geoMatrix.resize((size_t)validSats, 4);
00401 int counter(0);
00402 for ( iter=vCosines.begin(); iter!=vCosines.end(); iter++ )
00403 {
00404 geoMatrix(counter,0) = (*iter)[0];
00405 geoMatrix(counter,1) = (*iter)[1];
00406 geoMatrix(counter,2) = (*iter)[2];
00407
00408 geoMatrix(counter,3) = 1.0;
00409 counter++;
00410 }
00411
00412 if (validSats >= 4)
00413 {
00414 validData = true;
00415 }
00416
00417 return validSats;
00418
00419 }
00420 catch(Exception& e)
00421 {
00422 GPSTK_RETHROW(e);
00423 }
00424
00425 }
00426
00427
00428
00429
00430
00431 int ModeledReferencePR::Compute( const DayTime& Tr,
00432 Vector<SatID>& Satellite,
00433 Vector<double>& Pseudorange,
00434 const XvtStore<SatID>& Eph )
00435 throw(Exception)
00436 {
00437
00438
00439 Vector<double> vectorBIAS(1, 0.0);
00440
00441
00442 return ModeledReferencePR::Compute( Tr,
00443 Satellite,
00444 Pseudorange,
00445 Eph,
00446 vectorBIAS );
00447
00448 }
00449
00450
00451
00452
00453
00454 int ModeledReferencePR::Compute( const DayTime& Tr,
00455 Vector<SatID>& Satellite,
00456 Vector<double>& Pseudorange,
00457 const XvtStore<SatID>& Eph,
00458 TropModel *pTropModel )
00459 throw(Exception)
00460 {
00461
00462
00463 Vector<double> vectorBIAS(1, 0.0);
00464
00465
00466 return ModeledReferencePR::Compute( Tr,
00467 Satellite,
00468 Pseudorange,
00469 Eph,
00470 vectorBIAS,
00471 pTropModel );
00472
00473 }
00474
00475
00476
00477
00478
00479 int ModeledReferencePR::Compute( const DayTime& Tr,
00480 Vector<SatID>& Satellite,
00481 Vector<double>& Pseudorange,
00482 const XvtStore<SatID>& Eph,
00483 const Vector<double>& extraBiases,
00484 IonoModelStore *pIonoModel )
00485 throw(Exception)
00486 {
00487
00488
00489 TropModel *pTropModel=NULL;
00490
00491
00492 return ModeledReferencePR::Compute( Tr,
00493 Satellite,
00494 Pseudorange,
00495 Eph,
00496 extraBiases,
00497 pTropModel,
00498 pIonoModel );
00499
00500 }
00501
00502
00503
00504
00505
00506 int ModeledReferencePR::Compute( const DayTime& Tr,
00507 Vector<SatID>& Satellite,
00508 Vector<double>& Pseudorange,
00509 const XvtStore<SatID>& Eph,
00510 IonoModelStore *pIonoModel )
00511 throw(Exception)
00512 {
00513
00514
00515 Vector<double> vectorBIAS(1, 0.0);
00516 TropModel *pTropModel=NULL;
00517
00518
00519 return ModeledReferencePR::Compute( Tr,
00520 Satellite,
00521 Pseudorange,
00522 Eph,
00523 vectorBIAS,
00524 pTropModel,
00525 pIonoModel );
00526
00527 }
00528
00529
00530
00531
00532
00533 int ModeledReferencePR::Compute( const DayTime& Tr,
00534 Vector<SatID>& Satellite,
00535 Vector<double>& Pseudorange,
00536 const XvtStore<SatID>& Eph,
00537 TropModel *pTropModel,
00538 IonoModelStore *pIonoModel )
00539 throw(Exception)
00540 {
00541
00542
00543 Vector<double> vectorBIAS(1, 0.0);
00544
00545
00546 return ModeledReferencePR::Compute( Tr,
00547 Satellite,
00548 Pseudorange,
00549 Eph,
00550 vectorBIAS,
00551 pTropModel,
00552 pIonoModel );
00553
00554 }
00555
00556
00557
00558
00559
00560
00561
00562
00563
00564
00565
00566
00567
00568
00569
00570
00571
00572
00573
00574 int ModeledReferencePR::Compute( const DayTime& Tr,
00575 SatID& Satellite,
00576 double& Pseudorange,
00577 const XvtStore<SatID>& Eph,
00578 const double& extraBiases,
00579 TropModel *pTropModel,
00580 IonoModelStore *pIonoModel )
00581 throw(Exception)
00582 {
00583
00584
00585 Vector<SatID> vectorSV(1, Satellite);
00586 Vector<double> vectorPR(1, Pseudorange);
00587 Vector<double> vectorBIAS(1, extraBiases);
00588
00589
00590 return ModeledReferencePR::Compute( Tr,
00591 vectorSV,
00592 vectorPR,
00593 Eph,
00594 vectorBIAS,
00595 pTropModel,
00596 pIonoModel );
00597
00598 }
00599
00600
00601
00602
00603
00604
00605
00606
00607
00608 satTypeValueMap& ModeledReferencePR::processModel( const DayTime& time,
00609 satTypeValueMap& gData )
00610 throw(Exception)
00611 {
00612
00613 Vector<SatID> Vsat = gData.getVectorOfSatID();
00614 Vector<double> Vprange=gData.getVectorOfTypeID(getDefaultObservable());
00615
00616 try
00617 {
00618
00619
00620
00621 Compute( time,
00622 Vsat,
00623 Vprange,
00624 (*(getDefaultEphemeris())),
00625 extraBiases,
00626 getDefaultTropoModel(),
00627 getDefaultIonoModel() );
00628
00629
00630
00631 SatIDSet rejectedSet;
00632 for (size_t i = 0; i<rejectedSV.size(); ++i)
00633 {
00634 rejectedSet.insert(rejectedSV[i]);
00635 }
00636
00637
00638 gData.removeSatID(rejectedSet);
00639
00640
00641 gData.insertTypeIDVector(TypeID::prefitC, prefitResiduals);
00642 gData.insertTypeIDVector(TypeID::rho, geometricRho);
00643 gData.insertTypeIDVector(TypeID::dtSat, svClockBiases);
00644 gData.insertTypeIDVector(TypeID::rel, svRelativity);
00645 gData.insertTypeIDVector(TypeID::tropoSlant, tropoCorrections);
00646 gData.insertTypeIDVector(TypeID::elevation, elevationSV);
00647 gData.insertTypeIDVector(TypeID::azimuth, azimuthSV);
00648
00649
00650
00651 TypeID ionoDelayType, instDelayType;
00652
00653 switch ( getDefaultObservable().type )
00654 {
00655
00656 case TypeID::C1:
00657 case TypeID::P1:
00658 ionoDelayType = TypeID::ionoL1;
00659 instDelayType = TypeID::instC1;
00660 break;
00661
00662 case TypeID::C2:
00663 case TypeID::P2:
00664 ionoDelayType = TypeID::ionoL2;
00665 instDelayType = TypeID::instC2;
00666 break;
00667
00668 case TypeID::C5:
00669 ionoDelayType = TypeID::ionoL5;
00670 instDelayType = TypeID::instC5;
00671 break;
00672
00673 case TypeID::C6:
00674 ionoDelayType = TypeID::ionoL6;
00675 instDelayType = TypeID::instC6;
00676 break;
00677
00678 case TypeID::C7:
00679 ionoDelayType = TypeID::ionoL7;
00680 instDelayType = TypeID::instC7;
00681 break;
00682
00683 case TypeID::C8:
00684 ionoDelayType = TypeID::ionoL8;
00685 instDelayType = TypeID::instC8;
00686 break;
00687
00688 default:
00689 ionoDelayType = TypeID::ionoL1;
00690 instDelayType = TypeID::instC1;
00691
00692 }
00693
00694
00695
00696 gData.insertTypeIDVector(ionoDelayType, ionoCorrections);
00697
00698
00699 if( useTGD )
00700 {
00701 gData.insertTypeIDVector(instDelayType, svTGD);
00702 }
00703
00704
00705
00706 TypeIDSet tSet;
00707 tSet.insert(TypeID::dx);
00708 tSet.insert(TypeID::dy);
00709 tSet.insert(TypeID::dz);
00710 tSet.insert(TypeID::cdt);
00711 gData.insertMatrix(tSet, geoMatrix);
00712
00713
00714 return gData;
00715
00716 }
00717 catch(Exception& e)
00718 {
00719 GPSTK_RETHROW(e);
00720 }
00721
00722 }
00723
00724
00725
00726
00727 void ModeledReferencePR::init()
00728 {
00729
00730 setInitialRxPosition();
00731 geometricRho(0);
00732 svClockBiases(0);
00733 svXvt(0);
00734 svTGD(0);
00735 svRelativity(0);
00736 ionoCorrections(0);
00737 tropoCorrections(0);
00738 modeledPseudoranges(0);
00739 prefitResiduals(0);
00740 extraBiases(0);
00741 availableSV(0);
00742 rejectedSV(0);
00743
00744 }
00745
00746
00747
00748
00749
00750
00751
00752
00753 int ModeledReferencePR::setInitialRxPosition( const double& aRx,
00754 const double& bRx,
00755 const double& cRx,
00756 Position::CoordinateSystem s,
00757 GeoidModel *geoid )
00758 {
00759
00760 try
00761 {
00762 Position rxpos(aRx, bRx, cRx, s, geoid);
00763 setInitialRxPosition(rxpos);
00764 return 0;
00765 }
00766 catch(GeometryException& e)
00767 {
00768 return -1;
00769 }
00770
00771 }
00772
00773
00774
00775
00776 int ModeledReferencePR::setInitialRxPosition(const Position& RxCoordinates)
00777 {
00778
00779 try
00780 {
00781 rxPos = RxCoordinates;
00782 return 0;
00783 }
00784 catch(GeometryException& e)
00785 {
00786 return -1;
00787 }
00788
00789 }
00790
00791
00792
00793
00794 int ModeledReferencePR::setInitialRxPosition()
00795 {
00796
00797 try
00798 {
00799 Position rxpos(0.0, 0.0, 0.0, Position::Cartesian, NULL);
00800 setInitialRxPosition(rxpos);
00801 return 0;
00802 }
00803 catch(GeometryException& e)
00804 {
00805 return -1;
00806 }
00807
00808 }
00809
00810
00811
00812
00813 double ModeledReferencePR::getTropoCorrections( TropModel *pTropModel,
00814 double elevation )
00815 {
00816
00817 double tropoCorr(0.0);
00818
00819 try
00820 {
00821 tropoCorr = pTropModel->correction(elevation);
00822
00823 if(!(pTropModel->isValid()))
00824 {
00825 tropoCorr = 0.0;
00826 }
00827 }
00828 catch(TropModel::InvalidTropModel& e)
00829 {
00830 tropoCorr = 0.0;
00831 }
00832
00833 return tropoCorr;
00834
00835 }
00836
00837
00838
00839
00840 double ModeledReferencePR::getIonoCorrections( IonoModelStore *pIonoModel,
00841 DayTime Tr,
00842 Geodetic rxGeo,
00843 double elevation,
00844 double azimuth )
00845 {
00846
00847 double ionoCorr(0.0);
00848
00849 try
00850 {
00851 ionoCorr = pIonoModel->getCorrection(Tr, rxGeo, elevation, azimuth);
00852 }
00853 catch(IonoModelStore::NoIonoModelFound& e)
00854 {
00855 ionoCorr = 0.0;
00856 }
00857
00858 return ionoCorr;
00859
00860 }
00861
00862
00863
00864
00865 double ModeledReferencePR::getTGDCorrections( DayTime Tr,
00866 const XvtStore<SatID>& Eph,
00867 SatID sat )
00868 {
00869
00870 try
00871 {
00872 const GPSEphemerisStore& bce =
00873 dynamic_cast<const GPSEphemerisStore&>(Eph);
00874
00875 const EngEphemeris& eph = bce.findEphemeris(sat,Tr);
00876
00877 return ( bce.findEphemeris(sat,Tr).getTgd() * C_GPS_M );
00878
00879 }
00880 catch(...)
00881 {
00882 return 0.0;
00883 }
00884
00885 }
00886
00887
00888 }