Add PseudorangeFactorArm and DifferentialPseudorangeFactorArm by inuex35 · Pull Request #2447 · borglab/gtsam

inuex35 · 2026-03-02T13:52:58Z

Description:
This PR introduces new factors to the navigation module that support lever arm compensation for GNSS sensors.

Previously, PseudorangeFactor required the antenna's absolute ECEF position. The new PseudorangeFactorArm factors accept a Pose3 (body pose in the navigation frame) and a static lever arm vector in the body frame. This allows the factor to dynamically compute the antenna position based on the body's rotation.

…nuex35/gtsam into feature/PseudorangeFactorLever

Copilot

Pull request overview

This PR adds lever-arm–aware GNSS pseudorange factors to the navigation module, enabling pseudorange residuals to be expressed on a body Pose3 while computing the antenna position via a fixed body-frame lever arm.

Changes:

Added PseudorangeFactorArm and DifferentialPseudorangeFactorArm (Pose3 + lever arm) implementations.
Exposed the new factors to the language wrappers via navigation.i.
Added unit tests validating construction, Jacobians, lever-arm behavior, printing, and equality.

Reviewed changes

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

File	Description
gtsam/navigation/PseudorangeFactor.h	Declares the new lever-arm pseudorange factor classes, API docs, traits, and serialization.
gtsam/navigation/PseudorangeFactor.cpp	Implements error evaluation/Jacobians, print/equals, and constructors for the new factors.
gtsam/navigation/navigation.i	Adds SWIG interface declarations for the new factor types.
gtsam/navigation/tests/testPseudorangeFactor.cpp	Adds test coverage for the new lever-arm pseudorange factors.

Copilot · 2026-03-04T04:29:07Z

gtsam/navigation/PseudorangeFactor.h

+  /** default constructor - only use for serialization */
+  PseudorangeFactorArm() : bL_(0, 0, 0) {}
+
+  virtual ~PseudorangeFactorArm() = default;


PseudorangeFactorArm() only initializes bL_; the inherited PseudorangeBase members (pseudorange_, satPos_, satClkBias_) remain uninitialized. Since print()/equals() access these members (and the tests construct a default factor and call print()/equals()), this is undefined behavior. Initialize the PseudorangeBase subobject in the default constructor (or add default member initializers in PseudorangeBase) so default-constructed factors are safe for these operations.

Copilot · 2026-03-04T04:29:07Z

gtsam/navigation/PseudorangeFactor.h

+  typedef DifferentialPseudorangeFactorArm This;
+
+  /** default constructor - only use for serialization */
+  DifferentialPseudorangeFactorArm() : bL_(0, 0, 0) {}


DifferentialPseudorangeFactorArm() only initializes bL_; the inherited PseudorangeBase members (pseudorange_, satPos_, satClkBias_) remain uninitialized. Because print()/equals() read these fields (and the tests default-construct this factor and call those methods), this is undefined behavior. Initialize the PseudorangeBase subobject (or provide default member initializers) in the default constructor.

Suggested change

DifferentialPseudorangeFactorArm() : bL_(0, 0, 0) {}

DifferentialPseudorangeFactorArm()

: PseudorangeBase{0.0, Point3(0, 0, 0), 0.0}, bL_(0, 0, 0) {}

Copilot · 2026-03-04T04:29:07Z

gtsam/navigation/tests/testPseudorangeFactor.cpp

+  // Clock bias derivative should always be speed-of-light in vacuum:
+  EXPECT_DOUBLES_EQUAL(Hbias.norm(), 299792458.0, 1e-9);
+}


These tests compare Hbias.norm() to the speed of light with an extremely tight absolute tolerance (1e-9) on a ~3e8 value. The Frobenius norm uses a sqrt and can introduce tiny rounding differences; this risks flaky failures on some platforms/build flags. Prefer asserting directly on Hbias(0,0) (or use a looser tolerance / relative comparison).

Copilot · 2026-03-04T04:29:07Z

gtsam/navigation/tests/testPseudorangeFactor.cpp

+  EXPECT(!Hcorrection.array().isNaN().any());
+  EXPECT_DOUBLES_EQUAL(Hbias.norm(), 299792458.0, 1e-9);
+  EXPECT_DOUBLES_EQUAL(Hcorrection(0, 0), -1.0, 1e-9);


These tests compare Hbias.norm() to the speed of light with an extremely tight absolute tolerance (1e-9) on a ~3e8 value. Since the norm involves a sqrt, small floating-point rounding differences can make this flaky. Prefer checking Hbias(0,0) directly (or relax the tolerance / use relative error).

Copilot · 2026-03-04T04:29:08Z

gtsam/navigation/PseudorangeFactor.cpp

+    if (range < std::numeric_limits<double>::epsilon()) {
+      *H_nTb = Matrix16::Zero();
+    } else {


std::numeric_limits<double>::epsilon() is used here in the new code path, but this translation unit does not include <limits> directly. It likely compiles today via transitive includes, but it’s brittle; add an explicit #include <limits> in this .cpp (or the relevant header) to make the dependency explicit.

dellaert · 2026-03-07T14:45:44Z

@masoug Could you take a look on whether this fits in the pseudo-range factor hierarchy cleanly?

- Initialize PseudorangeBase in default constructors of PseudorangeFactorArm and DifferentialPseudorangeFactorArm to avoid undefined behavior - Replace Hbias.norm() with Hbias(0, 0) in tests to avoid floating-point issues with sqrt on large values like the speed of light - Add explicit #include <limits> in PseudorangeFactor.cpp for std::numeric_limits usage https://claude.ai/code/session_01WwBj5et3656PootjzHLNEJ

masoug · 2026-03-07T15:16:20Z

@masoug Could you take a look on whether this fits in the pseudo-range factor hierarchy cleanly?

I think this is fine for the time being. Eventually we'll need to re-work the PseudorangeFactor API as more features and capability get added (e.g. different frequencies, yes/no lever arms, carrier vs code measurements, non-US GNSS satellites, etc...) in the future. So if API stability is a critical priority, we'll want to do that refactor here and now. Otherwise, I'm happy with the proposed changes in this PR, and we'll cross the PseudorangeFactor API bridge when we get there.

inuex35 · 2026-03-07T22:45:10Z

Hi — I was working on RTK support in GTSAM but ended up stalling out. I'd love to see native RTK processing land in GTSAM eventually.
I've been experimenting in this repo: https://github.com/inuex35/pyins
Happy to help with implementation if there's interest!

inuex35 · 2026-03-08T04:21:17Z

Hi, I realized the current pseudorange factor is computed in ECEF, but when combining it with IMU or other sensors, it would be more convenient to use NED or ENU due to the complexity of IMU preintegration.

dellaert · 2026-03-08T15:14:33Z

@masoug please discuss :-) I think ECEF might be more native to GPS, and if you want NED or ENU you have the additional burden to choose that - which we might not want to have as a prerequisite to using these factors.

varunagrawal · 2026-03-09T19:02:27Z

Minor nitpick: the naming convention usually puts Factor at the end, so the factor should be called PseudorangeArmFactor.

dellaert · 2026-03-09T20:04:28Z

Minor nitpick: the naming convention usually puts Factor at the end, so the factor should be called PseudorangeArmFactor.

Even better, PseudoRangeArmFactor

masoug · 2026-03-10T06:17:26Z

@masoug please discuss :-) I think ECEF might be more native to GPS, and if you want NED or ENU you have the additional burden to choose that - which we might not want to have as a prerequisite to using these factors.

Sorry for the late reply, had a bunch of work/life stuff, not to mention Claude Code is quite the rabbit hole 🐰

I concur with @dellaert; GNSS itself is unaware of attitude because its primary focus is point positioning. Therefore, concepts like ENU/NED aren't involved in pseudorange/carrier-phase equations. Instead, users are responsible for choosing the frame convention for their particular application.

The original intention I had in my head was for users to explicitly model SE(3) body-frame and R^3 antenna-frame variables as two distinct nodes linked by a stiff pose-point-factor lever-arm constraint. This decouples GNSS point-positioning from application-specific attitude estimation, allowing flexibility for arbitrary frame conventions encoded in that pose-point factor. But I can see the convenience that comes from native lever-arm support in the pseudorange factors, so maybe the pose3 pseudorange lever-arm factor should have a "ENU" or "NED" option to specify a body-frame convention for applying the offset?

dellaert · 2026-03-10T18:53:41Z

Well, ENU and NED are navigation frame conventions, whereas the lever-arm is a body-frame quantity, right? Let me go check the code. @inuex35 please resolve the copilot comments that you have addressed and also please do the requested name change, then we can run CI.

Address PR borglab#2447 review feedback from Copilot

dellaert · 2026-03-11T15:36:46Z

This is awesome, but before we merge, I think the frame naming should be clarified.

Using nTb / nRb suggests a generic “navigation frame”, but in both factors the satellite position is explicitly ECEF, so the pose/translation side is effectively expected to be ECEF as well. In GTSAM, n often reads like a local navigation frame such as NED or ENU, which makes this easy to misread.

Would it make sense to rename these to something more explicit, for example eTb / eRb, or even ecef_T_body / ecef_R_body? That would make it much more obvious that:

the body pose supplied to this factor is expected in ECEF,
the satellite position is in ECEF, and
any conversion to or from a local navigation frame remains the caller’s responsibility.

I think that expectation should also be stated directly in the class and constructor documentation, since otherwise a reader could reasonably assume this accepts an arbitrary local navigation frame.

Address PR review feedback from dellaert: rename frame variables to make explicit that the body pose and satellite position are in ECEF, not a generic navigation frame. Also update documentation to clarify that local frame (NED/ENU) conversion is the caller's responsibility. https://claude.ai/code/session_01GqWXxUkLXHV89MnocPt1ws

…Sv2B Rename frame variables from nTb/nRb to ecef_T_body/ecef_R_body

inuex35 · 2026-03-12T00:50:27Z

I have updated the implementation to work in ECEF coordinates for now. However, if future sensor fusion with cameras and LiDAR is planned, I believe local-frame Jacobians will be necessary.
I also aligned the factor names with the existing convention, where GpsFactor is named GpsFactorArm.

dellaert

Thanks, my comments have been addressed. I also agree with the naming.

inuex35 · 2026-03-19T12:51:56Z

Could you merge this when you get a chance? I'm planning to work on lever arm compensation and ENU/NED coordinate support next.

inuex35 added 3 commits March 2, 2026 22:04

add leverarm for pseudorange

a964316

Merge branch 'feature/PseudorangeFactorLever' of https://github.com/i…

43549d0

…nuex35/gtsam into feature/PseudorangeFactorLever

add differential

d65e989

inuex35 changed the title ~~Feature/pseudorange factor lever~~ Add PseudorangeFactorArm and DifferentialPseudorangeFactorArm Mar 2, 2026

dellaert requested a review from Copilot March 4, 2026 04:24

Copilot started reviewing on behalf of dellaert March 4, 2026 04:25 View session

Copilot AI reviewed Mar 4, 2026

View reviewed changes

inuex35 and others added 2 commits March 6, 2026 09:33

Merge branch 'borglab:develop' into feature/PseudorangeFactorLever

607db7b

fix

b5e4a93

Merge branch 'borglab:develop' into feature/PseudorangeFactorLever

0e5ba10

Merge pull request #4 from inuex35/claude/address-pr-feedback-uyVmT

f3fc4fc

Address PR borglab#2447 review feedback from Copilot

claude and others added 2 commits March 12, 2026 00:40

Merge pull request #5 from inuex35/claude/add-pseudorange-lever-arm-M…

8396ac5

…Sv2B Rename frame variables from nTb/nRb to ecef_T_body/ecef_R_body

dellaert approved these changes Mar 13, 2026

View reviewed changes

dellaert merged commit 75b4219 into borglab:develop Mar 19, 2026
32 checks passed

inuex35 deleted the feature/PseudorangeFactorLever branch March 20, 2026 10:58

inuex35 mentioned this pull request Mar 23, 2026

Add optional ecef_T_nav to PseudorangeFactorArm for GNSS/IMU coupling #2477

Open

	DifferentialPseudorangeFactorArm() : bL_(0, 0, 0) {}
	DifferentialPseudorangeFactorArm()
	: PseudorangeBase{0.0, Point3(0, 0, 0), 0.0}, bL_(0, 0, 0) {}

Conversation

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

Uh oh!

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Reviewed changes

Uh oh!

Copilot AI Mar 4, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Mar 4, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Mar 4, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Mar 4, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Mar 4, 2026

Choose a reason for hiding this comment

Uh oh!

dellaert commented Mar 7, 2026

Uh oh!

masoug commented Mar 7, 2026

Uh oh!

inuex35 commented Mar 7, 2026

Uh oh!

inuex35 commented Mar 8, 2026

Uh oh!

dellaert commented Mar 8, 2026

Uh oh!

varunagrawal commented Mar 9, 2026

Uh oh!

dellaert commented Mar 9, 2026

Uh oh!

masoug commented Mar 10, 2026

Uh oh!

dellaert commented Mar 10, 2026

Uh oh!

dellaert commented Mar 11, 2026

Uh oh!

inuex35 commented Mar 12, 2026

Uh oh!

dellaert left a comment

Choose a reason for hiding this comment

Uh oh!

inuex35 commented Mar 19, 2026

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

6 participants

inuex35 commented Mar 2, 2026 •

edited

Loading