00001 #pragma ident "$Id: ModeledReferencePR.cpp 3140 2012-06-18 15:03:02Z susancummins $"
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 CommonTime& 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<CommonTime> 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 tempIono = getIonoCorrections( pIonoModel,
00313 Tr,
00314 rxPos,
00315 cerange.elevationGeodetic,
00316 cerange.azimuthGeodetic );
00317 };
00318
00319 tempModeledPR = tempPR + tempTrop + tempIono;
00320
00321
00322
00323 if (i <= eN )
00324 {
00325 tempModeledPR += extraBiases(i);
00326 };
00327
00328
00329
00330 if(useTGD)
00331 {
00332 tempTGD = getTGDCorrections(Tr, Eph, Satellite[i]);
00333 tempModeledPR += tempTGD;
00334 }
00335
00336 tempPrefit = Pseudorange[i] - tempModeledPR;
00337
00338
00339 vGeometricRho.push_back(cerange.rawrange);
00340 vClock.push_back(cerange.svclkbias);
00341 vXvt.push_back(cerange.svPosVel);
00342 vTxTime.push_back(cerange.transmit);
00343 vTGD.push_back(tempTGD);
00344
00345 vRel.push_back(-cerange.relativity);
00346 vIono.push_back(tempIono);
00347 vTrop.push_back(tempTrop);
00348 vObservedPR.push_back(Pseudorange[i]);
00349 vModeledPR.push_back(tempModeledPR);
00350 vPrefit.push_back(tempPrefit);
00351 vElevation.push_back(cerange.elevationGeodetic);
00352 vAzimuth.push_back(cerange.azimuthGeodetic);
00353 vAvailableSV.push_back(Satellite[i]);
00354 vCosines.push_back(cerange.cosines);
00355
00356
00357 validSats += 1;
00358
00359 }
00360 catch(InvalidRequest& e)
00361 {
00362
00363
00364 vRejectedSV.push_back(Satellite[i]);
00365 continue;
00366 }
00367 catch(...)
00368 {
00369 Exception unknownEx( "An unknown exception has happened in \
00370 ModeledReferencePR object." );
00371 GPSTK_THROW(unknownEx);
00372 }
00373
00374 }
00375
00376
00377 rejectedSV = vRejectedSV;
00378 availableSV = vAvailableSV;
00379 geometricRho = vGeometricRho;
00380 svClockBiases = vClock;
00381 svXvt = vXvt;
00382 svTxTime = vTxTime;
00383 svTGD = vTGD;
00384 svRelativity = vRel;
00385 ionoCorrections = vIono;
00386 tropoCorrections = vTrop;
00387 observedPseudoranges = vObservedPR;
00388 modeledPseudoranges = vModeledPR;
00389 prefitResiduals = vPrefit;
00390 elevationSV = vElevation;
00391 azimuthSV = vAzimuth;
00392
00393
00394
00395 geoMatrix.resize((size_t)validSats, 4);
00396 int counter(0);
00397 for ( iter=vCosines.begin(); iter!=vCosines.end(); iter++ )
00398 {
00399 geoMatrix(counter,0) = (*iter)[0];
00400 geoMatrix(counter,1) = (*iter)[1];
00401 geoMatrix(counter,2) = (*iter)[2];
00402
00403 geoMatrix(counter,3) = 1.0;
00404 counter++;
00405 }
00406
00407 if (validSats >= 4)
00408 {
00409 validData = true;
00410 }
00411
00412 return validSats;
00413
00414 }
00415 catch(Exception& e)
00416 {
00417 GPSTK_RETHROW(e);
00418 }
00419
00420 }
00421
00422
00423
00424
00425
00426 int ModeledReferencePR::Compute( const CommonTime& Tr,
00427 Vector<SatID>& Satellite,
00428 Vector<double>& Pseudorange,
00429 const XvtStore<SatID>& Eph )
00430 throw(Exception)
00431 {
00432
00433
00434 Vector<double> vectorBIAS(1, 0.0);
00435
00436
00437 return ModeledReferencePR::Compute( Tr,
00438 Satellite,
00439 Pseudorange,
00440 Eph,
00441 vectorBIAS );
00442
00443 }
00444
00445
00446
00447
00448
00449 int ModeledReferencePR::Compute( const CommonTime& Tr,
00450 Vector<SatID>& Satellite,
00451 Vector<double>& Pseudorange,
00452 const XvtStore<SatID>& Eph,
00453 TropModel *pTropModel )
00454 throw(Exception)
00455 {
00456
00457
00458 Vector<double> vectorBIAS(1, 0.0);
00459
00460
00461 return ModeledReferencePR::Compute( Tr,
00462 Satellite,
00463 Pseudorange,
00464 Eph,
00465 vectorBIAS,
00466 pTropModel );
00467
00468 }
00469
00470
00471
00472
00473
00474 int ModeledReferencePR::Compute( const CommonTime& Tr,
00475 Vector<SatID>& Satellite,
00476 Vector<double>& Pseudorange,
00477 const XvtStore<SatID>& Eph,
00478 const Vector<double>& extraBiases,
00479 IonoModelStore *pIonoModel )
00480 throw(Exception)
00481 {
00482
00483
00484 TropModel *pTropModel=NULL;
00485
00486
00487 return ModeledReferencePR::Compute( Tr,
00488 Satellite,
00489 Pseudorange,
00490 Eph,
00491 extraBiases,
00492 pTropModel,
00493 pIonoModel );
00494
00495 }
00496
00497
00498
00499
00500
00501 int ModeledReferencePR::Compute( const CommonTime& Tr,
00502 Vector<SatID>& Satellite,
00503 Vector<double>& Pseudorange,
00504 const XvtStore<SatID>& Eph,
00505 IonoModelStore *pIonoModel )
00506 throw(Exception)
00507 {
00508
00509
00510 Vector<double> vectorBIAS(1, 0.0);
00511 TropModel *pTropModel=NULL;
00512
00513
00514 return ModeledReferencePR::Compute( Tr,
00515 Satellite,
00516 Pseudorange,
00517 Eph,
00518 vectorBIAS,
00519 pTropModel,
00520 pIonoModel );
00521
00522 }
00523
00524
00525
00526
00527
00528 int ModeledReferencePR::Compute( const CommonTime& Tr,
00529 Vector<SatID>& Satellite,
00530 Vector<double>& Pseudorange,
00531 const XvtStore<SatID>& Eph,
00532 TropModel *pTropModel,
00533 IonoModelStore *pIonoModel )
00534 throw(Exception)
00535 {
00536
00537
00538 Vector<double> vectorBIAS(1, 0.0);
00539
00540
00541 return ModeledReferencePR::Compute( Tr,
00542 Satellite,
00543 Pseudorange,
00544 Eph,
00545 vectorBIAS,
00546 pTropModel,
00547 pIonoModel );
00548
00549 }
00550
00551
00552
00553
00554
00555
00556
00557
00558
00559
00560
00561
00562
00563
00564
00565
00566
00567
00568
00569 int ModeledReferencePR::Compute( const CommonTime& Tr,
00570 SatID& Satellite,
00571 double& Pseudorange,
00572 const XvtStore<SatID>& Eph,
00573 const double& extraBiases,
00574 TropModel *pTropModel,
00575 IonoModelStore *pIonoModel )
00576 throw(Exception)
00577 {
00578
00579
00580 Vector<SatID> vectorSV(1, Satellite);
00581 Vector<double> vectorPR(1, Pseudorange);
00582 Vector<double> vectorBIAS(1, extraBiases);
00583
00584
00585 return ModeledReferencePR::Compute( Tr,
00586 vectorSV,
00587 vectorPR,
00588 Eph,
00589 vectorBIAS,
00590 pTropModel,
00591 pIonoModel );
00592
00593 }
00594
00595
00596
00597
00598
00599
00600
00601
00602
00603 satTypeValueMap& ModeledReferencePR::processModel( const CommonTime& time,
00604 satTypeValueMap& gData )
00605 throw(Exception)
00606 {
00607
00608 Vector<SatID> Vsat = gData.getVectorOfSatID();
00609 Vector<double> Vprange=gData.getVectorOfTypeID(getDefaultObservable());
00610
00611 try
00612 {
00613
00614
00615
00616 Compute( time,
00617 Vsat,
00618 Vprange,
00619 (*(getDefaultEphemeris())),
00620 extraBiases,
00621 getDefaultTropoModel(),
00622 getDefaultIonoModel() );
00623
00624
00625
00626 SatIDSet rejectedSet;
00627 for (size_t i = 0; i<rejectedSV.size(); ++i)
00628 {
00629 rejectedSet.insert(rejectedSV[i]);
00630 }
00631
00632
00633 gData.removeSatID(rejectedSet);
00634
00635
00636 gData.insertTypeIDVector(TypeID::prefitC, prefitResiduals);
00637 gData.insertTypeIDVector(TypeID::rho, geometricRho);
00638 gData.insertTypeIDVector(TypeID::dtSat, svClockBiases);
00639 gData.insertTypeIDVector(TypeID::rel, svRelativity);
00640 gData.insertTypeIDVector(TypeID::tropoSlant, tropoCorrections);
00641 gData.insertTypeIDVector(TypeID::elevation, elevationSV);
00642 gData.insertTypeIDVector(TypeID::azimuth, azimuthSV);
00643
00644
00645
00646 TypeID ionoDelayType, instDelayType;
00647
00648 switch ( getDefaultObservable().type )
00649 {
00650
00651 case TypeID::C1:
00652 case TypeID::P1:
00653 ionoDelayType = TypeID::ionoL1;
00654 instDelayType = TypeID::instC1;
00655 break;
00656
00657 case TypeID::C2:
00658 case TypeID::P2:
00659 ionoDelayType = TypeID::ionoL2;
00660 instDelayType = TypeID::instC2;
00661 break;
00662
00663 case TypeID::C5:
00664 ionoDelayType = TypeID::ionoL5;
00665 instDelayType = TypeID::instC5;
00666 break;
00667
00668 case TypeID::C6:
00669 ionoDelayType = TypeID::ionoL6;
00670 instDelayType = TypeID::instC6;
00671 break;
00672
00673 case TypeID::C7:
00674 ionoDelayType = TypeID::ionoL7;
00675 instDelayType = TypeID::instC7;
00676 break;
00677
00678 case TypeID::C8:
00679 ionoDelayType = TypeID::ionoL8;
00680 instDelayType = TypeID::instC8;
00681 break;
00682
00683 default:
00684 ionoDelayType = TypeID::ionoL1;
00685 instDelayType = TypeID::instC1;
00686
00687 }
00688
00689
00690
00691 gData.insertTypeIDVector(ionoDelayType, ionoCorrections);
00692
00693
00694 if( useTGD )
00695 {
00696 gData.insertTypeIDVector(instDelayType, svTGD);
00697 }
00698
00699
00700
00701 TypeIDSet tSet;
00702 tSet.insert(TypeID::dx);
00703 tSet.insert(TypeID::dy);
00704 tSet.insert(TypeID::dz);
00705 tSet.insert(TypeID::cdt);
00706 gData.insertMatrix(tSet, geoMatrix);
00707
00708
00709 return gData;
00710
00711 }
00712 catch(Exception& e)
00713 {
00714 GPSTK_RETHROW(e);
00715 }
00716
00717 }
00718
00719
00720
00721
00722 void ModeledReferencePR::init()
00723 {
00724
00725 setInitialRxPosition();
00726 geometricRho(0);
00727 svClockBiases(0);
00728 svXvt(0);
00729 svTGD(0);
00730 svRelativity(0);
00731 ionoCorrections(0);
00732 tropoCorrections(0);
00733 modeledPseudoranges(0);
00734 prefitResiduals(0);
00735 extraBiases(0);
00736 availableSV(0);
00737 rejectedSV(0);
00738
00739 }
00740
00741
00742
00743
00744
00745
00746
00747
00748 int ModeledReferencePR::setInitialRxPosition( const double& aRx,
00749 const double& bRx,
00750 const double& cRx,
00751 Position::CoordinateSystem s,
00752 EllipsoidModel *ell,
00753 ReferenceFrame frame )
00754 {
00755
00756 try
00757 {
00758 Position rxpos(aRx, bRx, cRx, s, ell, frame);
00759 setInitialRxPosition(rxpos);
00760 return 0;
00761 }
00762 catch(GeometryException& e)
00763 {
00764 return -1;
00765 }
00766
00767 }
00768
00769
00770
00771
00772 int ModeledReferencePR::setInitialRxPosition(const Position& RxCoordinates)
00773 {
00774
00775 try
00776 {
00777 rxPos = RxCoordinates;
00778 return 0;
00779 }
00780 catch(GeometryException& e)
00781 {
00782 return -1;
00783 }
00784
00785 }
00786
00787
00788
00789
00790 int ModeledReferencePR::setInitialRxPosition()
00791 {
00792
00793 try
00794 {
00795 Position rxpos(0.0, 0.0, 0.0, Position::Cartesian, NULL);
00796 setInitialRxPosition(rxpos);
00797 return 0;
00798 }
00799 catch(GeometryException& e)
00800 {
00801 return -1;
00802 }
00803
00804 }
00805
00806
00807
00808
00809 double ModeledReferencePR::getTropoCorrections( TropModel *pTropModel,
00810 double elevation )
00811 {
00812
00813 double tropoCorr(0.0);
00814
00815 try
00816 {
00817 tropoCorr = pTropModel->correction(elevation);
00818
00819 if(!(pTropModel->isValid()))
00820 {
00821 tropoCorr = 0.0;
00822 }
00823 }
00824 catch(TropModel::InvalidTropModel& e)
00825 {
00826 tropoCorr = 0.0;
00827 }
00828
00829 return tropoCorr;
00830
00831 }
00832
00833
00834
00835
00836 double ModeledReferencePR::getIonoCorrections( IonoModelStore *pIonoModel,
00837 CommonTime Tr,
00838 Position rxGeo,
00839 double elevation,
00840 double azimuth,
00841 IonoModel::Frequency freq )
00842 {
00843
00844 double ionoCorr(0.0);
00845
00846 try
00847 {
00848 ionoCorr = pIonoModel->getCorrection( Tr, rxGeo, elevation,
00849 azimuth, freq );
00850 }
00851 catch(IonoModelStore::NoIonoModelFound& e)
00852 {
00853 ionoCorr = 0.0;
00854 }
00855
00856 return ionoCorr;
00857
00858 }
00859
00860
00861
00862
00863 double ModeledReferencePR::getTGDCorrections( CommonTime Tr,
00864 const XvtStore<SatID>& Eph,
00865 SatID sat )
00866 {
00867
00868 try
00869 {
00870 const GPSEphemerisStore& bce =
00871 dynamic_cast<const GPSEphemerisStore&>(Eph);
00872
00873 const EngEphemeris& eph = bce.findEphemeris(sat,Tr);
00874
00875 return ( bce.findEphemeris(sat,Tr).getTgd() * C_MPS );
00876
00877 }
00878 catch(...)
00879 {
00880 return 0.0;
00881 }
00882
00883 }
00884
00885
00886 }