Simple GNSS Pseudorange Factor

[masoug]

Hi! This PR proposes a simple PseudorangeFactor that provides position (and clock timing) constraints from GNSS satellite data to GTSAM estimation problems. So far, the simple pseudorange factor focuses on just clock errors, and omits atmospheric effects. Furthermore, it is designed for code-phase measurements only, so it does not leverage high-precision carrier-phase data from the receiver. Despite that, this simple model already provides decent positioning accuracy on the scale of a city block, and my hope is to contribute several follow-up PRs that gradually improve accuracy/precision with better factor models and examples.

A basic demonstration of PseudorangeFactor is provided in the SinglePointPositioningExample.ipynb notebook. It captures ZOA1's position to ~33 meters of accuracy:

I'd like to get some feedback on this proposal while I continue working on the unit tests in the background. I think a family of pseudorange factors is interesting because it enables tightly-coupled GNSS factor graphs, which are more robust compared to GPSFactors. Individual pseudoranges provide fine-grain outlier rejection while offering partial observability when < 4 satellites are visible. Complex multi-station differential-GNSS positioning graphs are also enabled by pseudorange factors.

Anyway, this is my first GTSAM contribution, so please let me know if I'm missing any convention/formatting in my PR. Otherwise, I'm looking forward to exploring precise GNSS positioning with factor graphs!

[dellaert]

@masoug This is awesome !!! I made a couple of comments. We definitely need the derivatives tested. Also, GDCM uses specific naming conventions as detailed, among others, here: .github/copilot-instructions.md

I will trigger a co-pilot request and he probably will have a lot of comments about that kind of stuff.

I especially liked your Python notebook example. I'm a bit sad we need to calculate the satellite positions by calling on a different library, but I acknowledge it's not easy to reimplement that stuff. I'm also wondering whether we could batch up range measurements at a particular time, so that if we have measurements to 12 satellites, we only create one factor with a 12x3 Jacobian and 12x1 Jacobian - If that makes sense.

33 meters seems large, but I guess it's only 4 satellites. Do you have any idea how the error decreases if you use more satellites?

PS I was the external examiner on a tightly coupled GPS thesis, a long time ago, pre GTSAM even: link. This PR is a cool first step to actually recreating that thesis in GTSAM :-).

[Copilot]

Pull request overview

This PR introduces a new PseudorangeFactor for GNSS positioning in GTSAM, enabling tightly-coupled GNSS factor graphs using raw pseudorange measurements from satellites. The implementation focuses on code-phase measurements with clock error correction, omitting atmospheric effects for simplicity. A Jupyter notebook example demonstrates single-point positioning with ~33 meters accuracy at the ZOA1 CORS station.

Changes:

• Adds PseudorangeFactor class for modeling GNSS pseudorange measurements between receiver and satellite
• Includes Python wrapper bindings and comprehensive documentation notebooks
• Provides working example using real RINEX data from NOAA CORS network

Reviewed changes

Copilot reviewed 8 out of 8 changed files in this pull request and generated 8 comments.

Show a summary per file

File	Description
gtsam/navigation/PseudorangeFactor.h	Header file defining the PseudorangeFactor class with constructor, evaluateError, and serialization
gtsam/navigation/PseudorangeFactor.cpp	Implementation of PseudorangeFactor including pseudorange equation and Jacobian computation
gtsam/navigation/navigation.i	Python wrapper interface definitions for PseudorangeFactor
gtsam/navigation/tests/testPseudorangeFactor.cpp	Basic unit test for PseudorangeFactor constructor and zero-error case
gtsam/navigation/doc/PseudorangeFactor.ipynb	Documentation notebook explaining the factor with usage example
python/gtsam/examples/SinglePointPositioningExample.ipynb	Complete example demonstrating single-point positioning with real GNSS data
gtsam/navigation/navigation.md	Updated module documentation to include PseudorangeFactor reference
THANKS.md	Added contributor Sammy Guo to the acknowledgments

[dellaert]

By the way, we need a Python unit test as well if you add a wrapper. Otherwise there's a no guarantee that the wrapper will continue to work. After you add a Jacobian test I should be able to convert it to Python easily - although you might give some context to make sure it does so in the right style.

[dellaert]

By the way, I can't stress how cool this is :-)

[ProfFan]

I remember there is actually a repository on GitHub that does tightly coupled GPS with GTSAM, unfortunately I forgot where it is now...

[dellaert]

Some CI failures remain, please examine.

@ProfFan A FG-based entry was one of leading contenders in Google decimeter challenge:
https://www.kaggle.com/competitions/google-smartphone-decimeter-challenge/writeups/taro-1st-place-solution

[masoug]

Thanks everyone for the comments! I'm glad folks are interested in this topic too.

I'm a bit sad we need to calculate the satellite positions by calling on a different library, but I acknowledge it's not easy to reimplement that stuff.

Turns out, the satellite orbit algorithms are not difficult to compute, but they're also not really relevant to include within GTSAM at the moment. That said, there is potential in the future where pseudorange factors re-implement differentiable versions of orbital algorithms to refine satellite motion during optimization. But one step at a time 🦶 let's cross that bridge when we get there 🌉

I'm also wondering whether we could batch up range measurements at a particular time, so that if we have measurements to 12 satellites, we only create one factor with a 12x3 Jacobian and 12x1 Jacobian - If that makes sense.

That does make sense, however, I kept the factors separate to leave the door open for robust loss functions (like Huber). I'm wondering if the built-in M-estimators could function like RAIM to reject individual faulty satellites or RF interference, improving robustness against outliers. Do batch factors support fine-grain loss functions?

33 meters seems large, but I guess it's only 4 satellites. Do you have any idea how the error decreases if you use more satellites?

33 feels kinda high to me too, though a large part of the error might be explained by elevation uncertainty and the lack of atmospheric corrections. That said, the next experiment (PR) I'd like to try is a primitive differential GNSS factor where I use ZOA1 as a reference to correct single-point-solution of a nearby station like P222 with a more complex factor-graph network. I think most of the error should disappear at that point, but I'm not certain yet.

re more satellites: More satellites might help if the measured pseudoranges are not all biased by the same atmospheric effects. Without correcting for those common-mode errors, however, adding more pseudorange observations likely won't improve position accuracy.

A FG-based entry was one of leading contenders in Google decimeter challenge:

That's really cool 😎 Looks like GTSAM is a popular choice among the GNSS community, and I'm curious to see how far down the rabbit-hole this could go. It'd be super awesome if GTSAM can demonstrate survey-grade positioning accuracy/precision with large-scale factor graph networks fusing/estimating everything from IMU preintegration, satellite orbits, reference stations, carrier-phase measurements, doppler, SBAS, atmospheric effects, etc... Not to mention a whole other world of meteorology where GNSS factor-graph networks are used to measure the atmosphere. But I don't want to get ahead of myself--we'll see where we get through small, gradual steps 🚶

[dellaert]

LGTM, CI passes !

[varunagrawal]

This is indeed really cool. I did a project with Sehoon Ha on GNSS based urban localization for quadrupeds using GPS+RTK back in 2024. I wish this factor existed then.

I do have a Carrier Phase factor implemented in a private repository for that project if you're interested.
These are the results I got from walking around Georgia Tech's campus: