feat: double difference factor for rtk gnss by inuex35 · Pull Request #2502 · borglab/gtsam

inuex35 · 2026-04-13T22:28:44Z

Add DDPseudorangeFactor and DDCarrierPhaseFactor (with and without lever-arm)

Following up on #2447, this PR adds four double-differenced GNSS factors:

DDPseudorangeFactor – DD code pseudorange, takes a Point3 (ECEF antenna position)
DDPseudorangeFactorArm – same, but takes a Pose3 in the navigation frame + body-frame lever arm
DDCarrierPhaseFactor – DD carrier-phase, takes a Point3 (ECEF antenna position), with ambiguity variable N
DDCarrierPhaseFactorArm – same, but takes a Pose3 in the navigation frame + body-frame lever arm, with ambiguity variable N

Factors without the Arm suffix take an ECEF antenna position directly. The Arm variants take a Pose3 in the navigation frame and compute the antenna position via a fixed body-frame lever arm.

I'd appreciate feedback on whether this structure fits cleanly into the existing factor hierarchy. @masoug @dellaert

Related: #2447, #2477

Nonlinear DD and SD factors for GNSS RTK positioning: - DDPseudorangeFactor / DDCarrierPhaseFactor - DDPseudorangeFactorArm / DDCarrierPhaseFactorArm - PseudorangeFactor / CarrierPhaseFactor (undifferenced) - PseudorangeSDFactor / CarrierPhaseSDFactor - DifferentialPseudorangeFactor Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>

Move duplicated geodist(), OMGE, and C_LIGHT into a shared gtsam/navigation/GnssCommon.h header. This eliminates identical copies from DDPseudorangeFactor, DDPseudorangeFactorArm, DDCarrierPhaseFactor, and DDCarrierPhaseFactorArm. Also unifies the naming from CLIGHT (anonymous namespace) to gnss::C_LIGHT in both .cpp files. https://claude.ai/code/session_01Wqjya7ZCHeJFJiZ5Sxxgwt

Change all four DD factors (DDPseudorangeFactor, DDPseudorangeFactorArm, DDCarrierPhaseFactor, DDCarrierPhaseFactorArm) from (observed - model) to (model - observed), matching the convention used by the undifferenced PseudorangeFactor and CarrierPhaseFactor. Jacobian signs updated accordingly. https://claude.ai/code/session_01Wqjya7ZCHeJFJiZ5Sxxgwt

A localization test

Copilot

Pull request overview

Adds RTK-style double-difference GNSS factors (pseudorange + carrier phase) with optional lever-arm compensation, and introduces a shared GNSS utilities header to consolidate constants/geometry helpers.

Changes:

Add DDPseudorangeFactor / DDPseudorangeFactorArm (with optional ecef_T_nav) and corresponding unit tests.
Add CarrierPhaseFactor / CarrierPhaseFactorArm plus DDCarrierPhaseFactor / DDCarrierPhaseFactorArm and unit tests.
Introduce GnssCommon.h for shared GNSS constants and geodist() (Sagnac-corrected range), and wire constants into existing pseudorange code.

Reviewed changes

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

Show a summary per file

File	Description
gtsam/navigation/PseudorangeFactor.h	Adds DD pseudorange factor classes (point + lever-arm) inline.
gtsam/navigation/PseudorangeFactor.cpp	Switches to shared `C_LIGHT` constant; refactors placement of differential pseudorange implementation.
gtsam/navigation/GnssCommon.h	New shared GNSS constants + Sagnac-corrected `geodist()` utility.
gtsam/navigation/CarrierPhaseFactor.h	New carrier phase factors (undifferenced + DD, with lever-arm + optional `ecef_T_nav`).
gtsam/navigation/CarrierPhaseFactor.cpp	Implements undifferenced carrier phase factor logic + Jacobians.
gtsam/navigation/navigation.i	Exposes new factors to wrappers via SWIG interface.
gtsam/navigation/tests/testPseudorangeFactor.cpp	Adds unit tests for DD pseudorange factors (including lever-arm and `ecef_T_nav`).
gtsam/navigation/tests/testCarrierPhaseFactor.cpp	New unit tests for carrier phase factors (undifferenced + DD, lever-arm + `ecef_T_nav`).

varunagrawal

Some initial comments.

varunagrawal · 2026-04-15T19:27:15Z


 public:
-  // Provide access to the Matrix& version of evaluateError:
  using Base::evaluateError;


Do you need this? You've implemented evaluateError.

dellaert · 2026-04-15T20:45:09Z

Thanks for initial review, @varunagrawal . @masoug - would also appreciate your take on the implemented functionality, and how it relates.

@inuex35 please run make check locally and report on result in the PR comment. Make sure to compile with all warnings treated as errors. After you do this and address other comments above I'll run CI.

- Move heavy DD factor evaluateError(), print(), equals(), and ctor bodies from PseudorangeFactor.h / CarrierPhaseFactor.h into the matching .cpp files so headers stay light and follow the rest of the navigation module. - Refactor gnss::geodist() to take an OptionalJacobian<1, 3> for the range derivative w.r.t. the receiver position. The Jacobian now includes the Sagnac correction term, fixing the inconsistency between the modeled range and the previous Euclidean-only unit-vector Jacobian used by the DD factors. - Guard gnss::geodist() against zero distance (sat == rcv) to avoid NaNs. - DD pseudorange / carrier-phase factors (with and without lever arm) now consume the new Jacobian, so the chain rule through the Sagnac term is exact and consistent across all variants. - Drop the duplicate `#include <gtsam/navigation/CarrierPhaseFactor.h>` in navigation.i and fix the doxygen typo "= lambda * N).." in CarrierPhaseFactor.h. Local make check on the navigation tests passes: testPseudorangeFactor - no test failures testCarrierPhaseFactor - no test failures

- DDPseudorangeFactor -> DoubleDifferencePseudorangeFactor - DDPseudorangeFactorArm -> DoubleDifferencePseudorangeFactorArm - DDCarrierPhaseFactor -> DoubleDifferenceCarrierPhaseFactor - DDCarrierPhaseFactorArm -> DoubleDifferenceCarrierPhaseFactorArm Also expands "DD" in the doxygen class summaries to "Double-difference" so the class purpose is obvious without needing to know the GNSS jargon. navigation.i, the unit tests, and all internal references are updated. Local make check on the navigation tests still passes: testPseudorangeFactor - no test failures testCarrierPhaseFactor - no test failures

Claude/gtsam pr 2502 dc c aw

masoug

This is great! One humble request, if I may: Would you be willing to provide an end-to-end example of these carrier phase factors using data from physical receivers (similar to the other psuedorange factors)? I am especially interested in getting your thoughts on cm-scale positioning by some hybrid estimation between gtsam and a integer optimizer for ambiguity resolution.

inuex35 · 2026-04-16T15:09:48Z

@masoug you can try this. make sure there is no outlier or multipath detection.
you will achieve integer fix when it's open sky.
https://github.com/inuex35/gnss-gtsam-rtk

@masoug

- Consolidate PseudorangeBase and CarrierPhaseBase into a shared GnssMeasurementBase struct in GnssCommon.h (per @masoug's suggestion). Rename the measurement member from `pseudorange_`/`carrierPhase_` to the signal-agnostic `measurement_`. - Move gnss::geodist() implementation from the GnssCommon.h header into a new GnssCommon.cpp translation unit (per @masoug's suggestion). - Add testGnssCommon.cpp covering the Sagnac-corrected range, unit-vector normalization, the r==0 singularity handling, the analytic Jacobian vs. numerical derivative, and the Sagnac contribution to the Jacobian (per @masoug's request for unit/jacobian tests). - Expand DoubleDifferencePseudorangeFactor and DoubleDifferenceCarrierPhaseFactor docstrings to explain when these DD convenience factors should be used vs. composing undifferenced factors (per @masoug's question about purpose). - Remove the accidental duplicate `#include <gtsam/navigation/PseudorangeFactor.h>` in navigation.i (per Copilot). - Keep `using Base::evaluateError` with a short comment: removing it hides the NoiseModelFactorN convenience overloads that the unit tests rely on, matching the pattern used throughout gtsam/navigation.

…or-8Fpni Address PR borglab#2502 review feedback (round 2)

dellaert · 2026-04-21T17:12:41Z

@inuex35 Please resolve all comments above that you have addressed or respond to them if you don't think they are relevant. I will then do a final review - maybe I'll also push some commits onto your branch; I see that you've allowed that - which is good :-)

dellaert · 2026-04-23T01:52:38Z

Still seeing a bunch of comments that you did not resolve or respond to. I’m assuming you’re still looking at them :-). Please re-request a review for me when done.

Per review: carrier phase is conventionally quoted in cycles, but our API takes meters (= lambda * cycles). Rename constructor parameters to `...Meters` so the unit is unambiguous at the call site, without touching the shared base member `measurement_` (also used by PseudorangeFactor). - `measuredCarrierPhase` -> `measuredCarrierPhaseMeters` (CarrierPhaseFactor, CarrierPhaseFactorArm x2) - `cpRovRef/cpBaseRef/cpRovTarget/cpBaseTarget` -> `...Meters` (DoubleDifferenceCarrierPhaseFactor, DoubleDifferenceCarrierPhaseFactorArm x2) .h / .cpp / navigation.i all updated. No behavior change; both testCarrierPhaseFactor and testPseudorangeFactor pass.

inuex35 · 2026-04-23T14:06:12Z

@dellaert Thank you, maybe I finished the implementation. If you do not have any problem with this, please review again

dellaert · 2026-04-27T23:14:08Z

This is looking great, thanks! My remaining concern is mostly elegance/maintainability rather than the high-level functionality, in particular:

the DD pseudo-range and carrier-phase factors carry almost identical data and compute almost identical range/Jacobian terms, with carrier phase only adding the ambiguity term. Could we factor the shared DD observation/satellite/base-position data and the DD range/Jacobian computation into a small internal helper?
same goes for the arm variants: they repeat the same ecef_T_nav composition, antenna-position computation, and lever-arm Jacobian logic. Could this be pulled into a small helper, e.g. something that maps a Pose3 + lever arm + optional ecef_T_nav to antenna position and Jacobian?
finally, some similar comments to be made about the tests. Maybe some named namespaces with quantities used in several tests would eliminate copy/paste.

All these things would make the new API feel less copy/pasted and easier to maintain.

inuex35 · 2026-04-29T10:48:26Z

Thanks @dellaert, all three addressed:

Added gnss::DoubleDifferenceData in GnssCommon.h/.cpp — holds the rover/base observations + 4 satellite positions + base position, and exposes observed() and model(rcv, H_rcv) (Sagnac-corrected, with Jacobian). Both DD factors store one of these and just delegate; carrier-phase only adds the λ·(N_ref − N_target) term and the two ambiguity Jacobians.
Added gnss::LeverArm (lever arm + optional ecef_T_nav) with antennaPosition(pose, frame) and antennaPoseJacobian(H_antenna, frame). All four *Arm factors now share this for the Pose3 → antenna mapping and the chain rule through compose + lever arm.
Added gtsam/navigation/tests/gnssTestHelpers.h with the shared satellite/base/observation constants, lever arm, ecef_T_nav (Tokyo ENU origin), and λ_L1. Both test files use it, so each TEST body only specifies what's unique to that case.

Address PR review feedback that the DD pseudorange/carrier-phase factors and the *Arm variants are nearly copy-paste of each other. Extract the shared work into two helpers in `gtsam/navigation/GnssCommon`: - `gnss::DoubleDifferenceData` owns the four (rover/base x ref/target) observations, satellite ECEF positions at rover and base time, and the base station ECEF position. Provides `observed()` and `model(rcv, H)` which computes the DD geometric range with Sagnac correction and its 1x3 Jacobian. Used by DoubleDifferencePseudorangeFactor[Arm] and DoubleDifferenceCarrierPhaseFactor[Arm] -- the carrier-phase factor just adds the `+ lam * (ambRef - ambTarget)` term and its scalar Jacobians. - `gnss::LeverArm` owns `bL` + optional `ecef_T_nav`. Provides `antennaPosition(pose, &frame)` which composes the (optional) ecef_T_nav, applies the lever arm, and caches the rotation / compose-Jacobian needed later, plus `antennaPoseJacobian(H_antenna_1x3, frame)` which folds in the lever-arm chain rule and (optional) compose chain rule. Used by PseudorangeFactorArm, DifferentialPseudorangeFactorArm, CarrierPhaseFactorArm, DoubleDifferencePseudorangeFactorArm and DoubleDifferenceCarrierPhaseFactorArm so the `H_ecef = [u*-R*[bL]_x | u*R]; *H_pose = has_nav ? H_ecef * H_compose : H_ecef` block lives in one place. Each *Arm factor now stores a single `gnss::LeverArm arm_` member instead of separate `bL_` / `ecef_T_nav_` fields; the public `leverArm()` / `ecefTnav()` accessors and serialization NVP tags are preserved so the navigation.i bindings and existing serialization streams continue to work. DD factors store a single `gnss::DoubleDifferenceData dd_` member; the per-field NVP tags from the original layout (`prRovRef_`, `satRefRov_`, etc.) are kept for serialization compatibility. testPseudorangeFactor.cpp and testCarrierPhaseFactor.cpp share a new header `gnssTestHelpers.h` that holds the previously copy-pasted `makeEcefTnav()`, the L1 wavelength, the speed of light, the `SinglePointPositioningExample` sample (sat / receiver / clock), the DD shared geometry constants (kBasePos, kSatRefRov, ..., kTruePos), the Tokyo ENU origin, and a `geodistRange()` convenience. Tests now say e.g. `sample::kSatPos`, `dd::kTruePos`, `tokyo::ecefTnav()` instead of duplicating the numeric literals. All 30 navigation tests pass (including testSerializationNavigation). https://claude.ai/code/session_01HR3meboi8CCNsqntZzVxXH

dellaert

Awesome, LGTM!

inuex35 and others added 5 commits April 13, 2026 23:36

Merge branch 'borglab:develop' into dd-rtk-factor

280563a

Merge pull request #10 from inuex35/claude/review-dd-rtk-factor-KUU7y

f62a5f8

A localization test

dellaert requested a review from Copilot April 15, 2026 16:51

Copilot started reviewing on behalf of dellaert April 15, 2026 16:52 View session

Copilot AI reviewed Apr 15, 2026

View reviewed changes

varunagrawal reviewed Apr 15, 2026

View reviewed changes

claude and others added 3 commits April 15, 2026 23:58

Merge pull request #12 from inuex35/claude/gtsam-pr-2502-dcCAw

357c133

Claude/gtsam pr 2502 dc c aw

masoug reviewed Apr 16, 2026

View reviewed changes

Comment thread gtsam/navigation/CarrierPhaseFactor.h Outdated

Comment thread gtsam/navigation/CarrierPhaseFactor.h Outdated

Comment thread gtsam/navigation/CarrierPhaseFactor.h

Comment thread gtsam/navigation/GnssCommon.h Outdated

Comment thread gtsam/navigation/GnssCommon.h Outdated

claude and others added 2 commits April 16, 2026 23:56

Merge pull request #13 from inuex35/claude/add-double-difference-fact…

e216663

…or-8Fpni Address PR borglab#2502 review feedback (round 2)

inuex35 force-pushed the dd-rtk-factor branch from 6d03d86 to 685d7b7 Compare April 29, 2026 10:55

dellaert approved these changes May 1, 2026

View reviewed changes

dellaert merged commit e0b21bd into borglab:develop May 1, 2026
32 checks passed

inuex35 deleted the dd-rtk-factor branch May 4, 2026 01:55

Conversation

inuex35 commented Apr 13, 2026

Add DDPseudorangeFactor and DDCarrierPhaseFactor (with and without lever-arm)

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Reviewed changes

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

varunagrawal left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

varunagrawal Apr 15, 2026

Choose a reason for hiding this comment

Uh oh!

Uh oh!

dellaert commented Apr 15, 2026

Uh oh!

masoug left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

inuex35 commented Apr 16, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

dellaert commented Apr 21, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

dellaert commented Apr 23, 2026

Uh oh!

inuex35 commented Apr 23, 2026

Uh oh!

dellaert commented Apr 27, 2026

Uh oh!

inuex35 commented Apr 29, 2026

Uh oh!

dellaert left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

6 participants

inuex35 commented Apr 16, 2026 •

edited

Loading

dellaert commented Apr 21, 2026 •

edited

Loading