Question

Hi, I want do compute real time differential GPS solutions. It is for an outdoor robotic environment with small baselines (< 1km) I first wanted to calculate a position with a traditional (pseudorange only) method and then a solution including carrier phase information. I have two U-Blox AEK-4T (LEA-4T) Evaluation Kits for this purpose. I found the SolverPPP class for example but I'm not really sure about the whole process. Is the following idea right?

1. Convert ubx-messages (stream) to RINEX. Do I have to produce a RINEX file every step or do I have to append it every step to a single file?

2. Get SP3 Ephemerides. Where do I get them from? Can't I get it by the receivers itself?

3. Call the SolverPPP every step and use the latest RINEX files from both receivers

4. Repeat 1-3 for each cycle.

Is my idea right or did I completely misunderstand the whole thing?

Thanks for your help! Jakob

-- JakobR - 27 Nov 2008

Answer

ALERT! If you answer a question - or have a question you asked answered by someone - please remember to edit the page and set the status to answered. The status is in a drop-down list below the edit box.

Hi Jakob!

> Hi, I want do compute real time differential GPS solutions. It is for an outdoor robotic environment with small baselines (< 1km)...
> ... traditional (pseudorange only) method and then a solution including carrier phase information...
> ... I found the SolverPPP? class for example but I'm not really sure about the whole process. Is the following idea right?

> 1. Convert ubx-messages (stream) to RINEX. Do I have to produce a RINEX file every step or do I have to append it every step to a single file?

The former is possible, although far from optimal. RINEX is meant to be used for post-processing, and you're talking about real time here.

Therefore, I believe that the best path is to write a class that reads UBX protocol and feeds raw data directly to a "gnssRinex" object.

In order to achieve that, take as reference the operator that converts from "RinexObsData" (RINEX data) to "gnssRinex" (structured RINEX data). You'll find that code in files "DataStructures.hpp" and "DataStructures.cpp". The declaration is:

// Stream input for gnssRinex

std::istream& operator>>( std::istream& i, gnssRinex& f ) throw(FFStreamError, gpstk::StringUtils::StringException)

It is around line 1690 or so in "DataStructures.cpp".

> 2. Get SP3 Ephemerides. Where do I get them from? Can't I get it by the receivers itself?

You can't get SP3 ephemerides in real time because they are generated in post-process by IGS.

Said that, there are a couple things I must add:

  • IGS is now releasing "ultra-rapid" SP3-like products, where half of the data is indeed predictions. The performance of these orbits is VERY GOOD, specially in the first six hours of predictions. Clock predictions are as good (or as bad) as plain broadcast orbits.

  • For such a short baseline, you shouldn't have problems using broadcast orbits. Most satellite-related errors are cancelled when using DGPS (single differences taking a common satellite).

In summary: Use broadcast orbits.

> 3. Call the SolverPPP? every step and use the latest RINEX files from both receivers

Yes, you should use SolverPPP every (time) step.

However, beware of some important issues:

  • PPP processing is stand-alone, so you must add the appropriate DGPS-related classes. Please consult "examples/example7.cpp" to get the general process for code-based DGPS, and then read "examples/example8.cpp" to grasp the PPP-related issues.

It turns out that I recently presented paper at the European Space Agency about GPSTk and GNSS data structures, and I showed there an example VERY close to what you want to implement (but with a 115 km baseline). The paper is in the "Publications" section of GPSTk website and the appropriate links are:

http://www.gpstk.org/pub/Documentation/GPSTkPublications/Salazar-D-gpstk-high-accuracy.pdf (Paper)

http://www.gpstk.org/pub/Documentation/GPSTkPublications/presentation-Salazar-D-gpstk-high-accuracy.pdf (Presentation)

NOTE: Most of the examples and classes I'm talking about are found only in the current development branch of the GPSTk. You must go to the "Downloads" section of GPSTk website and follow the instructions labelled "Obtaining the Latest Development Version".

  • Also, take into account that by default PPP processing assumes that the receiver is static. Given that your application is kinematic, you MUST change the stochastic model associated to your rover coordinates. Please consult "examples/example8.cpp" and the aforementioned paper to know how to do that.

The safe bet is to set coordinates as white noise, but given that your robot should move relatively slow, and I guess your sampling rate is high ( 1 Hz? ), then three DIFFERENT (one for each dLat, dLon, dH) and CAREFULLY TUNED random walk stochastic models should yield better results.

Well, I guess this is it. I foresee that must of the work will be devoted to item 1).

Best regards,

Dago

-- DagobertoSalazar - 06 Jan 2009

Hi Dagoberto! Thanks for your very helpful answer! I followed your advice and wrote a class that feeds the UBX data into a gnssRinex object. It also feeds a GPSEphemerisStore and a RinexNavHeader object. When I look at example7 there is a synchronization done in line 374. Should my converter class fill a RinexObsStream as well in order to achieve that? Is there a paper or something describing this example? I'm not completely understanding everything done there. For example concerning the nominalPos that is needed to initialize the MOPSTropModel and the ModelObsFixedStation: Does it have to be a very precisely known position? As my application should be setup fast and be mobile in some way it would be a certain cutback. I wrote a function that gets the receiver calculated PR solution, is that enough to initialize those objects that need a position or do I have to go a completely other way for my solution? My application will be somehow similar to the Kinematic (precision-gps.org) application, do you know how that is done there? He's using double differences to calculate the rover position, do you have some advices for that?

Concerning the sampling rate, the LEA-4Ts give out raw observations up to 10 Hz.

In the next time I'll get to your paper and example8.

Best regards Jakob

-- JakobR - 28 Jan 2009

Hi Jakob!

> When I look at example7 there is a synchronization done in line 374.
> Should my converter class fill a RinexObsStream as well in order to
> achieve that?

Well, I guess there should be more efficient ways to achieve that. Take into account that the Synchronizer was designed for Rinex files, which means post-processing. I suggest you to use another strategy, more akin to your real-time data stream.

> Is there a paper or something describing this example?

Look at "Publications" link in GPSTk website for some papers and presentations about gnssRinex and the "GNSS Data Processing Paradigm". That's what is available right now.

> For example concerning the nominalPos that is needed to initialize
> the MOPSTropModel? and the ModelObsFixedStation?: Does it have to
> be a very precisely known position?

No, it is not necessary for it to be very precise. The most important part is the height, but take into account that this is a model, and some error is expected.

When using Neill tropospheric model, the hardest part to model (the drift of the "wet" troposphere) is estimated as an additional, random walk variable with a 1 cm*cm/h process spectral density.

You should update the nominal position with the last computed one, and it should be O.K. for your purposes. For initialization phase, PR solution is just fine.

> My application will be somehow similar to the Kinematic
> (precision-gps.org) application, do you know how that is done there?

I've not used Kinematic, but as they explain in:

http://precision-gps.org/Mathematics.htm

They initially use single differences, which is the same approach shown in the aforementioned GPSTk examples. Then, it seems they use some variable changes to get double differences.

However, they "float" the ambiguities, and this means that the full potential of double differences should not be achieved.

Therefore, in my opinion the performance of both methods should be roughly the same, with single-differencing phase-based positioning being a bit more simple.

I should note here, however, that it seems that you're currently missing bigger issues here: As far as I remember, the LEA-4Ts are single-frequency receivers, so you won't have available as many alternatives as it is usual for geodetic-grade positioning algorithms. You're helped by the short baseline (1 km), that allows you to remove most of the ionospheric and orbital effects, but there are other constraints.

Besides, when using differential phase-based positioning, you are usually computing:

  • dx, dy, dz (or dLat, dLon, dH) = 3
  • (dClockRef - dClockRobot) = 1
  • (zTropoRef - zTropoRobot) = 1 (*)
  • One phase ambiguity per each satellite in view = N

Thence, you'll have N+5 variables, and 2*N equations (N code-based and N phase-based), and you'll need at least 5 satellites in view, and a lot more than that to achieve a reasonable fast ambiguity convergence. I guess this could be your main problem.

I suggest you to implement first the simpler algorithms and progressively add more characteristics. Trying to add extra information to the positioning filter would be a good idea: accelerometers, and revolution-counters in the wheels of your robot should be considered, among other alternatives.

Regards,

Dago

(*) Indeed, these variables should be estimated separately, but this is a good approximation.

-- DagobertoSalazar - 29 Jan 2009

Thanks for your answer Dagoberto!

I'm getting further as far as I have a code based DGPS solution. In the meanwhile questions concerning the SolverPPP arose though. Is it suitable for single frequency at all? Yes, the U-Blox are only single frequency receivers. If yes do you have any advice for me as I surely have to change things concerning the linear combinations mentioned in you paper and in example10? Concerning the RTK approach: Are there already any examples available where I could get some inspiration? I've also read about a time difference approach here: http://www.weblab.dlr.de/rbrt/pdf/ION_07D23.pdf

Accuracy is decreasing with time but if it isn't too much work I should also give it a try as I should compare different approaches in my thesis. In the next time we have to decide if I should develop a gps only solution or stick with a simple solution and use sensor fusion using accelerometers, odometry,... What do you think would give the best results?

Regards Jakob

-- JakobR - 05 Mar 2009

Hi Jakob!

Class "SolverPPP" is suitable for single frequency, because it reads the GNSS Data Structure (GDS) just looking for "prefitC" and "prefitL" TypeID's, as well as geometry matrix parameters.

Therefore, what you must change is the way "prefitC" and "prefitL" are computed. For the first you may use the "LinearCombination" object called "c1Prefit", but for the later you must write your own "LinearCombination" object.

This is easy, and you can use "lcPrefit" for inspiration. Just make sure you use "TypeID::L1" instead of "TypeID::LC". Also, you must use the L1 wavelength to compute the "TypeID::windUp" coefficient.

Another important issue is cycle slip detection. Since you are using just one frequency, you must drop "LICSDetector" and "MWCSDetector", and use instead "OneFreqCSDetector", carefully tuned for your data rate.

Concerning RTK, the GPSTk currently doesn't provide classes to use it. The main issue is the ambiguity-fixing algorithm which is not trivial. I guess I'll implement Teunissen's Lambda algorithm, but it will take me at least a couple months. By the way, the article you referenced looks insteresting and I'll take a look.

Finally, I really believe that a "GPS + sensor fusion" approach, if properly implemented, will yield the best results, both in terms of accuracy and in terms of reliability. In the end, the navigation problem is an information-related problem: The more high-quality information is fed into the filter, the better the results obtained.

Best regards,

Dago

-- DagobertoSalazar - 08 Mar 2009

Hi Dagoberto! I made good success and my solution is already pretty good using only the white noise model for coordinates. As I'm working with 10 Hz data I had to adjust some models, maybe you can have a look at it and tell me if you think it's ok: markCSRover = OneFreqCSDetector(TypeID::L1, 31.0, 600, 4.5, 4.0); // maxWindowSize 600 for 10Hz data wnM = WhiteNoiseModel(20.0); markArc.setUnstablePeriod(0.2);

I'm not sure about the markArc unstablePeriod but the results are pretty good. Only very few outliers (about 20m) are still left when the rover has a bad sky view. I want to try out your new coordinate model possibilities but at the moment I'm using the SolverPPP in ECEF mode. I create a kml file for which I need coordinates though, how would you calculate the coordinates when having SolverPPP working in dLat/dLon/dH mode? Is there any gpstk method I could use for adding the relative distances to the base station coordinates or do I have to calculate it manually? As I have to explain the Kalman Filter used in the SolverPPP, is there any documentation on it or some short hints you could give me for that?

Thanks again for your excellent work! Jakob

-- JakobR - 03 May 2009

Hi Jakob!

It is nice to read that you are advancing. smile

Regarding your "tuning" of classes, I have some remarks:

markCSRover = OneFreqCSDetector(TypeID::L1, 31.0, 600, 4.5, 4.0);

You must change TypeID::L1 for TypeID::C1 (or TypeID::P1 ), because the first parameter is the "code type", instead of the "phase type".

The second parameter represents the "Maximum interval of time allowed between two successive epochs.". I believe your current value (31.0 s) is too high, because it means you are allowing about 310 missing samples before declaring a cycle slip. Let me suggest you to change it to something about 3 to 5 seconds.

wnM = WhiteNoiseModel(20.0);

It is my opinion that the sigma value for this model is a bit low. It suggest that you're quite sure (68% sure, indeed) that from one epoch to the next your rover doesn't move more than 20 meters.

The former is reasonable for your setup, but the problem arises at start up: Are you sure that your initial approximate position is within this uncertainty bound?. If it is not, your initialization phase will take more to converge. I'm assuming, by the way, that you are using your solution to update your position at each epoch, allowing the solution to drift to the appropriate value.

On the other hand, I think a "RandomWalkModel" is better for your setup: At the beginning, it initializes itself with a very high uncertainty value, and then adjust the uncertainty area according to the sample interval and rover maximum expected speed, leading to a stochastic model better adapted to your experiment.

By the way, the "SolverPPP" class has been updated and now you can assign a specific stochastic model to each coordinate.

markArc.setUnstablePeriod(0.2);

This means that, after finding a cycle slip, the affected satellite will be marked as "unstable" during 2 samples (at 10 Hz). This seems reasonable, but, of course, this only has sense if you have also used the method "SatArcMarker::setDeleteUnstableSats();" to delete unstable satellites.

Regarding your question about the solution, position and kml files, "Position" class provides you with some methods and operators to achieve that. I add a code snippet as example:


      //////// Initialization phase ////////

   cout << fixed << setprecision(8);   // Set a proper output format

   RinexNavData rNavData;              // Object to store Rinex navigation data
   GPSEphemerisStore bceStore;         // Object to store satellites ephemeris
   RinexNavHeader rNavHeader;          // Object to read the header of Rinex
                                       // navigation data files
   IonoModelStore ionoStore;           // Object to store ionospheric models
   IonoModel ioModel;                  // Declare a Ionospheric Model object

      // Create the input observation file stream
   RinexObsStream rin("nacc080a.09o.1s");

      // Create the input navigation file stream
   RinexNavStream rnavin("brdc0800.09n");

      // We need to read ionospheric parameters (Klobuchar model) from header
   rnavin >> rNavHeader;

      // Let's feed the ionospheric model (Klobuchar type) with data from the
      // Navigation file header
   ioModel.setModel(rNavHeader.ionAlpha, rNavHeader.ionBeta);
      // Beware: In this case, the same model will be used for the
      // full data span
   ionoStore.addIonoModel(DayTime::BEGINNING_OF_TIME, ioModel);

      // Storing the ephemeris in "bceStore"
   while (rnavin >> rNavData)
   {
      bceStore.addEphemeris(rNavData);
   }

   bceStore.SearchPast();  // This is the default

      // Station nominal position
   Position nominalPos(4709564.0, 162251.0, 4285937.0);

      // Declare SolverLMS object
   SolverLMS solver;


      // Declare a MOPSTropModel object, setting the defaults
   MOPSTropModel mopsTM( nominalPos.getAltitude(),
                         nominalPos.getGeodeticLatitude(),
                         80 );

      // Declare the modeler object, setting all the parameters in one pass
   ModelObs modelRef(nominalPos, ionoStore, mopsTM, bceStore, TypeID::C1);

   modelRef.Prepare( nominalPos );


      // This is the GNSS data structure that will hold all the
      // GNSS-related information
   gnssRinex gRin;

      //////// End of initialization phase ////////


      //////// Processing phase ////////

      // Loop over all data epochs
   while(rin >> gRin)
   {
      try
      {

            // This is the line that will process all the GPS data
         gRin >> modelRef >> solver;

      }
      catch(...)
      {
         cerr << "Exception at epoch: " << gRin.header.epoch << endl;
      }

      cout << gRin.header.epoch.DOYsecond() << " ";      // Output field #1

      Position solPos( (nominalPos.X() + solver.solution[0]),
                       (nominalPos.Y() + solver.solution[1]),
                       (nominalPos.Z() + solver.solution[2]) );

      cout << solPos.X() << "   ";                    // Output field #2
      cout << solPos.Y() << "   ";                    // Output field #3
      cout << solPos.Z() << "   ";                    // Output field #4
      cout << solPos.longitude() << "   ";            // Output field #5
      cout << solPos.geodeticLatitude() << "   ";     // Output field #6
      cout << solPos.height() << "   ";               // Output field #7
      cout << gRin.numSats() << "  ";                    // Output field #8

         // Update nominal position
      nominalPos = solPos;

         // Update tropospheric model
      mopsTM.setReceiverHeight( nominalPos.height() );
      mopsTM.setReceiverLatitude( nominalPos.geodeticLatitude() );

         // Update model
      modelRef.setDefaultTropoModel( mopsTM );
      modelRef.Prepare( nominalPos );

      cout << endl;
   }

Thanks for your comments and encouragement! wink

Regards,

Dago

-- DagobertoSalazar - 04 May 2009

Thanks for your quick answer! I finally found the real edit page so my questions should look a bit nicer now wink I will try out your suggestions as soon as I can get my hands on it.

There's still one question left though: I already saw the "SolverPPP" update concerning the model for each coordinate. My problem is now that I use the "SolverPPP" in ECEF mode. That also means that

wnM = WhiteNoiseModel(20.0);

is meant for X, Y, Z and not dLat, dLon, dH, right? So switching "SolverPPP" to dLat, dLon, dH mode should be my next step now but then my problem is how to get absolute rover coordinates for the kml file then. In your example

solver.solution[0..2]

gives X, Y, Z and not dLat, dLon, dH as it is the case for "SolverPPP" then, right?

-- JakobR - 05 May 2009

Hi again!

Regarding your question about stochastic model:

wnM = WhiteNoiseModel(20.0);

It is meant for dX, dY and dZ, not for dLat, dLon, dH, neither for X, Y, Z. Remember that the GNSS equation system is not linear, and thence we linearize it around an approximate position.

Therefore, when we do:

solver.solution[0..2]

We are getting dX, dY, dZ. With that information (and the approximate, nominal "Position" we linearized the problem around) we then compute the real, absolute "Position":

Position solPos( (nominalPos.X() + solver.solution[0]),
                 (nominalPos.Y() + solver.solution[1]),
                 (nominalPos.Z() + solver.solution[2]) );

From there, the "Position" class methods will provide the information needed for your .kml file:

cout << solPos.longitude() << "   ";
cout << solPos.geodeticLatitude() << "   ";
cout << solPos.height() << "   ";

I used this same code, with minor modifications, to get myself a .kml file, so you shouldn't have problems.

Cheers,

Dago

-- DagobertoSalazar - 05 May 2009

Sorry to disturb you again. Maybe I'm completely misunderstanding something so I try to explain my problem again. When I set the pppSolver do ENU mode like this:

pppSolver = new SolverPPP(true);

and define the coordinate models as you described it to take into account that the robot moves vertically only very slow

   WhiteNoiseModel wnHorizontalModel(2.0);
   RandomWalkModel rwVerticalModel(0.04);

   pppSolver.setXCoordinatesModel(&wnHorizontalModel);
   pppSolver.setYCoordinatesModel(&wnHorizontalModel);
   pppSolver.setZCoordinatesModel(&rwVerticalModel);

this will work on the dLat, dLon and dHs, right?

But now I need the geodetic coordinates for the kml file. In order to get them I would need something like ENU2XYZ to add the dLat, dLon and dHs to the nominal Rover position. But that doesn't exist. My question now is if I have to write this conversion by myself or if I'm misunderstanding something here?

When setting the solver to ECEF mode I can't model the fact that the robot moves very slow vertically, because

pppSolver.setZCoordinatesModel(&rwVerticalModel);

would work on dZ which isn't the same as dH, right?.

So I need to set it to ENU mode but then I don't know how to convert from ENU back to XYZ in order to get the absolute geodetic coordinates for the kml file...

-- JakobR - 07 May 2009

Hi!

Now I understand your doubt!.

In order to achieve a conversion from a topocentric system like ENU to a geocentric system like ECEF, you need to apply two rotations around 2nd and 3rd axes.

First, get your UEN correction vector (beware: it is not ENU, order is important for rotations):

      // Let's get UEN correction vector
   Triple delta( pppSolver.getSolution(TypeID::dH),
                 pppSolver.getSolution(TypeID::dLon),
                 pppSolver.getSolution(TypeID::dLat) );

Then, rotate your correction vector to ECEF. You'll need two consecutive rotations:

  • First: Rotation around your second axis ( R2 ), using +latitude.
  • Second: Rotation around your third axis ( R3 ), using -longitude.

      // Rotate delta to XYZ reference frame
   delta = ( delta.R2( nominalPos.geodeticLatitude() ) ).R3( -nominalPos.longitude() );

Declare a new position object using your new ECEF correction vector:

   Position newPos( nominalPos.X() + delta[0],
                    nominalPos.Y() + delta[1],
                    nominalPos.Z() + delta[2] );

Finally, get the Lat, Lon, Height values:

   cout << newPos.getGeodeticLatitude() << "  ";
   cout << newPos.getLongitude() << "  ";
   cout << newPos.getHeight() << "  ";

That is all. You should look up the rotation matrices (R1, R2, R3) in a geodesy book.

Cheers,

Dago

-- DagobertoSalazar - 07 May 2009

Thanks again Dagoberto! It works almost perfect now, I'm still impressed... The only problem left is when only few SVs are received, the solution sometimes goes crazy... It then takes a while until good results are reached again. Also when there are only 2 SVs it tries to give out a solution which results in outliers lying on a straight line. Is there anything I could do in order to prevent this? I'd rather give out solutions only when it makes sense or better said when enough SVs are received.

-- JakobR - 09 Jun 2009

Hi again! I think I found the solution to my problem. It was much worse in my recent observations than before so I took a close look at the observation files and found a bug in my UBX to RINEX converter. I didn't correct the IDs for geostationary SVs. I received PRN 124 and 126 in my recent observations which somehow led to other regular GPS SVs being deleted. I then fixed it by introducing SVs with PRN > 99 as geostationary SVs with ID = PRN - 100. I also calculate the cumulated variance (variances dLat + dLon + dH) now and introduced a threshold in my program. The big outliers can be eliminated with this method. I'm completely happy with my solution now, thanks again for your encouragement! I'll let you know when my professor decided about publishing or not. I think the UBX to RINEX / gnssRinex converter could be interesting for others, too.

-- JakobR - 19 Jun 2009

Hi Jakob!

Great news!. I'm glad to read that!.

Regarding your UBX to RINEX/gnssRinex converter, you could upload it to a public place so other people may take a lot at it.

Regards,

Dago

-- DagobertoSalazar - 17 Jul 2009

Dear Jakob,

I have a similar set up (but not real time). A set of L1 receivers for which I would like to have epoch-by-epoch positions in post processing. Would you be willing to share your code so I can modify it. Our data are in MACM format for which I have a RINEX convertor, but I would prefer to use your approach and write a direct-input module.

Thanks for your help! Sincerely, Sridhar.

-- SridharAnandakrishnan - 09 Oct 2009

Dear Sridhar,

sorry for the late response, I didn't check this thread in the last time.

I also implemented a post processing approach: A RINEX recorder for the LEA-4T and a postprocessing app. I want to publish my code soon. Although I have to clean it up before I can commit it somewhere. Anyway, I can send you the code already but it might take you a while to be able to compile it successfully. If you're still interested just give me your email.

Regards Jakob

-- JakobR - 15 Nov 2009

Jakob- yes, I am still interested (I have been out of the office for the last couple of months - collecting more data) so now I really need a good way to process them! Sincerely, Sridhar

-- SridharAnandakrishnan - 14 Jan 2010 No such template def TMPL:DEF{PROMPT:supportquery}
Topic revision: r34 - 28 Jan 2012, AdminRickMach
 

This site is powered by FoswikiCopyright © by the contributing authors. All material on this collaboration platform is the property of the contributing authors.
Ideas, requests, problems regarding Foswiki? Send feedback