00001
00002
00003
00004
00005
00006
00007
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
00033
00034
00035
00036
00037 #include <stdio.h>
00038 #include <math.h>
00039 #include "constants.h"
00040 #include "geodesy.h"
00041 #include "GNSS_RxData.h"
00042 #include "GNSS_Estimator.h"
00043 #include "GNSS_OptionFile.h"
00044
00045
00046 #ifndef WIN32
00047 #define _CRT_SECURE_NO_DEPRECATE
00048 #endif
00049
00050 using namespace GNSS;
00051
00052
00053
00054
00055
00056
00057 bool GetNextSetOfSynchronousMeasurements(
00058 GNSS_RxData &rxDataBase,
00059 bool &endOfStreamBase,
00060 GNSS_RxData &rxDataRover,
00061 bool &endOfStreamRover,
00062 bool &isSynchronized
00063 );
00064
00065
00066 int main( int argc, char* argv[] )
00067 {
00068 GNSS_RxData rxDataBase;
00069 GNSS_RxData rxDataRover;
00070 GNSS_Estimator Estimator;
00071 bool isValidPath;
00072 bool result;
00073 bool endOfStreamBase = false;
00074 bool endOfStreamRover = false;
00075 bool isSynchronized = false;
00076
00077 char supermsg[8192];
00078 unsigned nrBytesInBuffer = 0;
00079
00080 supermsg[0] = 0;
00081
00082
00083 unsigned i = 0;
00084 unsigned j = 0;
00085
00086
00087 double dT = 0.0;
00088 double time = 0.0;
00089 double time_prev = 0.0;
00090
00091 double start_time = 0.0;
00092 double end_time = 604800.0;
00093
00094 bool wasPositionComputed = false;
00095 bool wasVelocityComputed = false;
00096
00097 FILE *fid = NULL;
00098 #ifdef EXTRACTSVDATA
00099 FILE *svfid = NULL;
00100 unsigned short prn = 11;
00101 #endif
00102
00103 GNSS_OptionFile opt;
00104
00105 std::string OptionFilePath;
00106
00107 bool useLSQ = true;
00108 bool useEKF = false;
00109 bool useRTK = false;
00110 bool useRTKDD = false;
00111
00112 bool isAtFirstEpoch = true;
00113
00114 GNSS_structPVT firstPVT;
00115 GNSS_structPVT secondPVT;
00116 bool firstPVT_isSet = false;
00117
00118 try
00119 {
00120 printf( "\nUSAGE: EGNSS <option file path>\n" );
00121 if( argc != 2 )
00122 {
00123 return 0;
00124 }
00125 OptionFilePath = argv[1];
00126
00127 if( !opt.ReadAndInterpretOptions( OptionFilePath ) )
00128 {
00129 printf( "\nInvalid option file.\n" );
00130 return 1;
00131 }
00132
00133
00134 start_time = opt.m_StartTime.GPSWeek*SECONDS_IN_WEEK + opt.m_StartTime.GPSTimeOfWeek;
00135
00136
00137 end_time = opt.m_EndTime.GPSWeek*SECONDS_IN_WEEK + opt.m_EndTime.GPSTimeOfWeek;
00138
00139 if( opt.m_ProcessingMethod == "LSQ" )
00140 {
00141 useLSQ = true;
00142 useEKF = false;
00143 useRTK = false;
00144 }
00145 else if( opt.m_ProcessingMethod == "EKF" )
00146 {
00147 useLSQ = true;
00148 useEKF = true;
00149 useRTK = false;
00150 }
00151 else if( opt.m_ProcessingMethod == "RTK" )
00152 {
00153 useLSQ = true;
00154 useEKF = false;
00155 useRTK = true;
00156 }
00157 else if( opt.m_ProcessingMethod == "RTKDD" )
00158 {
00159 useLSQ = true;
00160 useEKF = false;
00161 useRTK = false;
00162 useRTKDD = true;
00163 }
00164
00165 if( opt.m_ProcessingMethod != "LSQ" )
00166 {
00167 Estimator.m_KF.alphaVn = opt.m_KalmanOptions.alphaVn;
00168 Estimator.m_KF.alphaVe = opt.m_KalmanOptions.alphaVe;
00169 Estimator.m_KF.alphaVup = opt.m_KalmanOptions.alphaVup;
00170 Estimator.m_KF.alphaClkDrift = opt.m_KalmanOptions.alphaClkDrift;
00171 Estimator.m_KF.sigmaVn = opt.m_KalmanOptions.sigmaVn;
00172 Estimator.m_KF.sigmaVe = opt.m_KalmanOptions.sigmaVe;
00173 Estimator.m_KF.sigmaVup = opt.m_KalmanOptions.sigmaVup;
00174 Estimator.m_KF.sigmaClkDrift = opt.m_KalmanOptions.sigmaClkDrift;
00175 }
00176
00177
00178 #ifndef _CRT_SECURE_NO_DEPRECATE
00179
00180 if( fopen_s( &fid, opt.m_OutputFilePath.c_str(), "w" ) != 0 )
00181 return 1;
00182 #else
00183 fid = fopen( opt.m_OutputFilePath.c_str(), "w" );
00184 if( fid == NULL )
00185 return 1;
00186 #endif
00187
00188
00189 #ifdef EXTRACTSVDATA
00190
00191
00192
00193 #ifndef _CRT_SECURE_NO_DEPRECATE
00194 if( fopen_s( &svfid, "prn.txt", "w" ) != 0 )
00195 return 1;
00196 #else
00197 svfid = fopen( "theprn.txt", "w" );
00198 if( svfid == NULL )
00199 return 1;
00200 #endif
00201
00202 #endif
00203
00204 if( opt.m_Reference.isValid )
00205 {
00206 if( opt.m_RINEXNavDataPath.length() != 0 )
00207 {
00208 result = rxDataBase.Initialize( opt.m_Reference.DataPath.c_str(), isValidPath, opt.m_Reference.DataType, opt.m_RINEXNavDataPath.c_str() );
00209 }
00210 else
00211 {
00212 result = rxDataBase.Initialize( opt.m_Reference.DataPath.c_str(), isValidPath, opt.m_Reference.DataType, NULL );
00213 }
00214 if( !result )
00215 {
00216 printf( "\nBad path to the reference station data.\n" );
00217 return 1;
00218 }
00219
00220 if( opt.m_klobuchar.isValid )
00221 {
00222 rxDataBase.m_klobuchar = opt.m_klobuchar;
00223 if( !opt.m_Reference.useIono )
00224 rxDataBase.m_DisableIonoCorrection = true;
00225 }
00226 else
00227 {
00228 rxDataBase.m_DisableIonoCorrection = true;
00229 }
00230 rxDataBase.m_elevationMask = opt.m_elevationMask*DEG2RAD;
00231 rxDataBase.m_locktimeMask = opt.m_locktimeMask;
00232 rxDataBase.m_cnoMask = opt.m_cnoMask;
00233
00234 if( !opt.m_Reference.useTropo )
00235 {
00236 rxDataBase.m_DisableTropoCorrection = true;
00237 }
00238
00239 result = rxDataBase.SetInitialPVT(
00240 opt.m_Reference.latitudeRads,
00241 opt.m_Reference.longitudeRads,
00242 opt.m_Reference.height,
00243 0.0, 0.0, 0.0,
00244 0.0, 0.0,
00245 0.0, 0.0, 0.0,
00246 0.0, 0.0, 0.0,
00247 100.0,
00248 10.0 );
00249 if( !result )
00250 return 1;
00251 }
00252
00253 if( !opt.m_Rover.isValid )
00254 {
00255 return 1;
00256 }
00257
00258 if( !opt.m_Reference.isValid && opt.m_RINEXNavDataPath.length() != 0 )
00259 {
00260
00261 result = rxDataRover.Initialize( opt.m_Rover.DataPath.c_str(), isValidPath, opt.m_Rover.DataType, opt.m_RINEXNavDataPath.c_str() );
00262 }
00263 else
00264 {
00265 result = rxDataRover.Initialize( opt.m_Rover.DataPath.c_str(), isValidPath, opt.m_Rover.DataType, NULL );
00266 }
00267 if( !result )
00268 return 1;
00269 if( opt.m_klobuchar.isValid )
00270 {
00271 rxDataRover.m_klobuchar = opt.m_klobuchar;
00272 if( !opt.m_Reference.useIono )
00273 rxDataRover.m_DisableIonoCorrection = true;
00274 }
00275 else
00276 {
00277 rxDataRover.m_DisableIonoCorrection = true;
00278 }
00279 rxDataRover.m_elevationMask = opt.m_elevationMask*DEG2RAD;
00280 rxDataRover.m_locktimeMask = opt.m_locktimeMask;
00281 rxDataRover.m_cnoMask = opt.m_cnoMask;
00282
00283 if( !opt.m_Rover.useTropo )
00284 {
00285 rxDataRover.m_DisableTropoCorrection = true;
00286 }
00287
00288
00289 result = rxDataRover.SetInitialPVT(
00290 opt.m_Rover.latitudeRads,
00291 opt.m_Rover.longitudeRads,
00292 opt.m_Rover.height,
00293 0.0, 0.0, 0.0,
00294 0.0, 0.0,
00295 opt.m_Rover.uncertaintyLatitudeOneSigma,
00296 opt.m_Rover.uncertaintyLongitudeOneSigma,
00297 opt.m_Rover.uncertaintyHeightOneSigma,
00298 1.0, 1.0, 1.0,
00299 1000.0,
00300 1.0e9 );
00301 if( !result )
00302 return 1;
00303
00304
00305 while( !endOfStreamRover )
00306 {
00307 if( !isAtFirstEpoch )
00308 {
00309 time_prev = rxDataRover.m_pvt.time.gps_week*SECONDS_IN_WEEK + rxDataRover.m_pvt.time.gps_tow;
00310 }
00311
00312 if( opt.m_Reference.isValid )
00313 {
00314 result = GetNextSetOfSynchronousMeasurements(
00315 rxDataBase,
00316 endOfStreamBase,
00317 rxDataRover,
00318 endOfStreamRover,
00319 isSynchronized );
00320 if( !result )
00321 return -1;
00322 }
00323 else
00324 {
00325 result = rxDataRover.LoadNext( endOfStreamRover );
00326 if( !result )
00327 return -1;
00328 }
00329
00330 if( endOfStreamRover )
00331 break;
00332
00333 if( opt.m_Reference.isValid )
00334 {
00335 if( endOfStreamBase )
00336 break;
00337 }
00338
00339 if( rxDataRover.m_nrValidObs == 0 )
00340 continue;
00341
00342
00343 time = rxDataRover.m_pvt.time.gps_week*SECONDS_IN_WEEK + rxDataRover.m_pvt.time.gps_tow;
00344 if( time < start_time )
00345 continue;
00346 if( time > end_time )
00347 break;
00348
00349
00350
00351 if( rxDataRover.m_pvt.time.gps_tow > 242005 )
00352 int ggg = 99;
00353
00354 if( rxDataRover.m_pvt.time.gps_tow > 353172 )
00355 int ggga = 99;
00356
00357
00358 if( isAtFirstEpoch )
00359 {
00360 isAtFirstEpoch = false;
00361 dT = 0.0;
00362 }
00363 else
00364 {
00365 dT = time - time_prev;
00366 if( dT < 0.0 )
00367 {
00368
00369 printf( "\ndT is negative!\n");
00370 return 1;
00371 }
00372 }
00373
00374
00375
00376 for( j = 0; j < opt.m_Rover.nrSatsToExclude; j++ )
00377 {
00378 for( i = 0; i < rxDataRover.m_nrValidObs; i++ )
00379 {
00380 if( rxDataRover.m_ObsArray[i].id == opt.m_Rover.satsToExclude[j] )
00381 {
00382 rxDataRover.m_ObsArray[i].flags.isNotUserRejected = 0;
00383 }
00384 }
00385 }
00386
00387 if( opt.m_Reference.isValid )
00388 {
00389
00390 for( j = 0; j < opt.m_Reference.nrSatsToExclude; j++ )
00391 {
00392 for( i = 0; i < rxDataBase.m_nrValidObs; i++ )
00393 {
00394 if( rxDataBase.m_ObsArray[i].id == opt.m_Reference.satsToExclude[j] )
00395 {
00396 rxDataBase.m_ObsArray[i].flags.isNotUserRejected = 0;
00397 }
00398 }
00399 }
00400 }
00401
00402
00403 if( useLSQ )
00404 {
00405 if( opt.m_Reference.isValid )
00406 {
00407 result = Estimator.PerformLeastSquares_8StatePVT(
00408 &rxDataRover,
00409 &rxDataBase,
00410 wasPositionComputed,
00411 wasVelocityComputed );
00412 if( !result )
00413 return -1;
00414 }
00415 else
00416 {
00417 result = Estimator.PerformLeastSquares_8StatePVT(
00418 &rxDataRover,
00419 NULL,
00420 wasPositionComputed,
00421 wasVelocityComputed );
00422 if( !result )
00423 return -1;
00424 }
00425
00426 if( !wasPositionComputed )
00427 continue;
00428
00429 if( !wasVelocityComputed && (useEKF || useRTK || useRTKDD) )
00430 {
00431
00432
00433 if( !firstPVT_isSet )
00434 {
00435 firstPVT = rxDataRover.m_pvt;
00436 firstPVT_isSet = true;
00437 continue;
00438 }
00439 else
00440 {
00441 secondPVT = rxDataRover.m_pvt;
00442
00443 double N = 0;
00444 double M = 0;
00445 double tmp_vn = 0;
00446 double tmp_ve = 0;
00447 double tmp_vup = 0;
00448 double tmp_clkdrift = 0;
00449 double deltaTime = (secondPVT.time.gps_week*SECONDS_IN_WEEK + secondPVT.time.gps_tow) -
00450 (firstPVT.time.gps_week*SECONDS_IN_WEEK + firstPVT.time.gps_tow);
00451
00452 if( deltaTime < 1e-04 )
00453 {
00454 firstPVT_isSet = false;
00455 continue;
00456 }
00457
00458 GEODESY_ComputeMeridianRadiusOfCurvature(
00459 GEODESY_REFERENCE_ELLIPSE_WGS84,
00460 rxDataRover.m_pvt.latitude,
00461 &M );
00462
00463 GEODESY_ComputePrimeVerticalRadiusOfCurvature(
00464 GEODESY_REFERENCE_ELLIPSE_WGS84,
00465 rxDataRover.m_pvt.latitude,
00466 &N );
00467
00468 tmp_vn = (secondPVT.latitude - firstPVT.latitude)*(M+secondPVT.height) / deltaTime;
00469 tmp_ve = (secondPVT.longitude - firstPVT.longitude)*((N+secondPVT.height)*cos(secondPVT.latitude)) / deltaTime;
00470 tmp_vup = (secondPVT.height - firstPVT.height) / deltaTime;
00471 tmp_clkdrift = (secondPVT.clockOffset - firstPVT.clockOffset) / deltaTime;
00472
00473 result = rxDataRover.UpdateVelocityAndClockDrift(
00474 tmp_vn,
00475 tmp_ve,
00476 tmp_vup,
00477 tmp_clkdrift,
00478 5.0*deltaTime,
00479 5.0*deltaTime,
00480 5.0*deltaTime,
00481 5.0*deltaTime
00482 );
00483 if( !result )
00484 return -1;
00485 wasVelocityComputed = true;
00486 }
00487 }
00488
00489 if( wasPositionComputed && wasVelocityComputed )
00490 {
00491 if( useEKF || useRTK )
00492 {
00493 useLSQ = false;
00494 result = Estimator.InitializeStateVarianceCovariance_8StatePVGM(
00495 rxDataRover.m_pvt.std_lat,
00496 rxDataRover.m_pvt.std_lon,
00497 rxDataRover.m_pvt.std_hgt,
00498 rxDataRover.m_pvt.std_vn,
00499 rxDataRover.m_pvt.std_ve,
00500 rxDataRover.m_pvt.std_vup,
00501 rxDataRover.m_pvt.std_clk,
00502 rxDataRover.m_pvt.std_clkdrift,
00503 Estimator.m_RTK.P );
00504
00505 if( !result )
00506 return 1;
00507 }
00508
00509 else if ( useRTKDD )
00510 {
00511 useLSQ = false;
00512 result = Estimator.InitializeStateVarianceCovariance_6StatePVGM(
00513 rxDataRover.m_pvt.std_lat,
00514 rxDataRover.m_pvt.std_lon,
00515 rxDataRover.m_pvt.std_hgt,
00516 rxDataRover.m_pvt.std_vn,
00517 rxDataRover.m_pvt.std_ve,
00518 rxDataRover.m_pvt.std_vup,
00519 Estimator.m_RTKDD.P );
00520 if( !result )
00521 return 1;
00522 }
00523 }
00524 }
00525 else if( useEKF )
00526 {
00527 result = Estimator.PredictAhead_8StatePVGM(
00528 rxDataRover,
00529 dT,
00530 Estimator.m_RTK.T,
00531 Estimator.m_RTK.Q,
00532 Estimator.m_RTK.P );
00533 if( !result )
00534 return 1;
00535
00536 if( opt.m_Reference.isValid )
00537 {
00538 result = Estimator.Kalman_Update_8StatePVGM(
00539 &rxDataRover,
00540 &rxDataBase,
00541 Estimator.m_RTK.P );
00542 if( !result )
00543 return -1;
00544 }
00545 else
00546 {
00547 result = Estimator.Kalman_Update_8StatePVGM(
00548 &rxDataRover,
00549 NULL,
00550 Estimator.m_RTK.P );
00551 if( !result )
00552 return -1;
00553 }
00554 }
00555 else if( useRTK )
00556 {
00557 result = Estimator.PredictAhead_8StatePVGM_Float(
00558 rxDataRover,
00559 dT,
00560 Estimator.m_RTK.T,
00561 Estimator.m_RTK.Q,
00562 Estimator.m_RTK.P );
00563 if( !result )
00564 return 1;
00565
00566 if( opt.m_Reference.isValid )
00567 {
00568 result = Estimator.Kalman_Update_8StatePVGM_SequentialMode_FloatSolution(
00569 &rxDataRover,
00570 &rxDataBase,
00571 Estimator.m_RTK.P );
00572 if( !result )
00573 return 1;
00574 }
00575 else
00576 {
00577 result = Estimator.Kalman_Update_8StatePVGM_SequentialMode_FloatSolution(
00578 &rxDataRover,
00579 NULL,
00580 Estimator.m_RTK.P );
00581 if( !result )
00582 return 1;
00583 }
00584 }
00585
00586 else if ( useRTKDD )
00587 {
00588 result = Estimator.PredictAhead_6StatePVGM_Float(
00589 rxDataRover,
00590 dT,
00591 Estimator.m_RTKDD.T,
00592 Estimator.m_RTKDD.Q,
00593 Estimator.m_RTKDD.P );
00594 if ( !result )
00595 return 1;
00596
00597 result = Estimator.Kalman_Update_6StatePVGM_FloatSolution(
00598 &rxDataRover,
00599 &rxDataBase,
00600 Estimator.m_RTKDD.P );
00601 if ( !result )
00602 return 1;
00603
00604 result = Estimator.FixAmbiguities();
00605 if ( !result )
00606 return 1;
00607 }
00608
00609
00610 printf( "%12.3lf %5d %d %d %d \n",
00611 rxDataRover.m_pvt.time.gps_tow,
00612 rxDataRover.m_pvt.time.gps_week,
00613 rxDataRover.m_pvt.nrPsrObsUsed,
00614 rxDataRover.m_pvt.nrDopplerObsUsed,
00615 rxDataRover.m_pvt.nrAdrObsUsed );
00616
00617
00618
00619
00620
00621
00622
00623
00624
00625
00626
00627
00628
00629
00630
00631 rxDataRover.m_prev_pvt = rxDataRover.m_pvt;
00632 rxDataBase.m_prev_pvt = rxDataBase.m_pvt;
00633
00634 fprintf( fid, "%12.4lf %4d %20.10lf %20.10lf %15.3lf %20.10lf %4d %10.4lf %10.4lf %10.4lf %20.10lf %10.4lf %10.4lf %10.4lf %10.4lf %10.2f %10.2f %10.2f\n",
00635 rxDataRover.m_pvt.time.gps_tow,
00636 rxDataRover.m_pvt.time.gps_week,
00637 rxDataRover.m_pvt.latitudeDegs,
00638 rxDataRover.m_pvt.longitudeDegs,
00639 rxDataRover.m_pvt.height,
00640 rxDataRover.m_pvt.clockOffset,
00641 rxDataRover.m_pvt.nrPsrObsUsed,
00642 rxDataRover.m_pvt.vn,
00643 rxDataRover.m_pvt.ve,
00644 rxDataRover.m_pvt.vup,
00645 rxDataRover.m_pvt.clockDrift,
00646 rxDataRover.m_pvt.std_lat,
00647 rxDataRover.m_pvt.std_lon,
00648 rxDataRover.m_pvt.std_hgt,
00649 rxDataRover.m_pvt.std_clk,
00650 rxDataRover.m_pvt.dop.hdop,
00651 rxDataRover.m_pvt.dop.vdop,
00652 rxDataRover.m_pvt.dop.tdop
00653 );
00654
00655 #ifdef EXTRACTSVDATA
00656 for( i = 0; i < rxDataRover.m_nrValidObs; i++ )
00657 {
00658 if( rxDataRover.m_ObsArray[i].id == prn &&
00659 rxDataRover.m_ObsArray[i].system == GNSS_GPS &&
00660 rxDataRover.m_ObsArray[i].freqType == GNSS_GPSL1
00661 )
00662 {
00663 fprintf( svfid, "%12.4lf %4d %10.1f %10.1f %10.1f %20.10lf %20.10lf %20.10lf %20.10lf\n",
00664 rxDataRover.m_pvt.time.gps_tow,
00665 rxDataRover.m_pvt.time.gps_week,
00666 rxDataRover.m_ObsArray[i].cno,
00667 rxDataRover.m_ObsArray[i].satellite.elevation*RAD2DEG,
00668 rxDataRover.m_ObsArray[i].satellite.azimuth*RAD2DEG,
00669 rxDataRover.m_ObsArray[i].psr_misclosure,
00670 rxDataRover.m_ObsArray[i].doppler_misclosure,
00671 rxDataRover.m_ObsArray[i].psr,
00672 rxDataRover.m_ObsArray[i].adr
00673 );
00674 }
00675 }
00676 #endif
00677 }
00678
00679
00680 fclose( fid );
00681 #ifdef EXTRACTSVDATA
00682 fclose(svfid);
00683 #endif
00684 }
00685 catch( MatrixException& matrixException )
00686 {
00687 printf( "%s", matrixException.GetExceptionMessage().c_str() );
00688 }
00689 catch ( ... )
00690 {
00691 printf( "\nCaught unknown exception\n" );
00692 }
00693
00694 return 0;
00695 }
00696
00697
00698
00699 bool GetNextSetOfSynchronousMeasurements(
00700 GNSS_RxData &rxDataBase,
00701 bool &endOfStreamBase,
00702 GNSS_RxData &rxDataRover,
00703 bool &endOfStreamRover,
00704 bool &isSynchronized
00705 )
00706 {
00707 bool result = false;
00708 double timeBase = 0;
00709 double timeRover = 0;
00710 double timeDiff = 0;
00711
00712 isSynchronized = false;
00713
00714 result = rxDataBase.LoadNext( endOfStreamBase );
00715 if( !result )
00716 return false;
00717 if( endOfStreamBase )
00718 return true;
00719
00720 result = rxDataRover.LoadNext( endOfStreamRover );
00721 if( !result )
00722 return false;
00723 if( endOfStreamRover )
00724 return true;
00725
00726 while( !endOfStreamBase && !endOfStreamRover )
00727 {
00728 timeBase = rxDataBase.m_pvt.time.gps_week*SECONDS_IN_WEEK + rxDataBase.m_pvt.time.gps_tow;
00729 timeRover = rxDataRover.m_pvt.time.gps_week*SECONDS_IN_WEEK + rxDataRover.m_pvt.time.gps_tow;
00730 timeDiff = timeBase - timeRover;
00731
00732 if( fabs(timeDiff) < 0.010 )
00733 {
00734 isSynchronized = true;
00735 break;
00736 }
00737
00738 if( timeDiff < 0 )
00739 {
00740
00741 result = rxDataBase.LoadNext( endOfStreamRover );
00742 if( !result )
00743 return 1;
00744 }
00745 else
00746 {
00747 result = rxDataRover.LoadNext( endOfStreamRover );
00748 if( !result )
00749 return 1;
00750 }
00751 }
00752
00753 return true;
00754 }
00755
00756
00757
00758
00759
00760
00761
00762
00763