navigation: add GNSS Doppler (range-rate) factors#2589
Open
inuex35 wants to merge 7 commits into
Open
Conversation
DopplerFactor relates a measured Doppler to receiver velocity and clock drift, reusing the gnss::geodist line-of-sight: error = e.(v_s - v_r) + c*(ddt_r - ddt_s) - (-lambda*Doppler) Keys: [velocity, clock drift]. The satellite velocity and LOS are held constant per factor (their dependence on receiver position is second order). ClockDriftFactor encodes the constant-velocity (constant-drift) clock model, linking two consecutive clock biases through the shared drift: error = bias(k) - bias(k-1) - drift*dt This is the scalar analog of the IMU position/velocity link (not a plain BetweenFactor, since the transition is non-identity). It lets Doppler-observed drift propagate into the clock bias used by the pseudorange/carrier factors. Adds Python bindings and a test (residual + numerical Jacobians for both). Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
The range-rate model omitted the time-derivative of the Sagnac range correction that RTKLIB's resdop() includes (a ~cm/s effect). Add it, split into a v_r-independent offset and a linear coefficient on the receiver velocity, both precomputed in the constructor from the satellite position/velocity and receiver position already passed in. No change to gnss::geodist (it handles the position/range Sagnac; the rate term lives in the velocity domain). - evaluateError: predicted rate += sagnacOffset_ + velSagnac_.v_r; velocity Jacobian becomes (velSagnac_ - e)^T. - Update print/equals/serialize for the two new members. - testDopplerFactor: fold the Sagnac rate into the reference value. Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
Add DopplerFactorArm, the lever-arm variant of DopplerFactor, and address
code-review findings.
DopplerFactorArm keys on [Pose3, velocity, clock drift]. Unlike the
pseudorange/carrier Arm factors (which correct antenna *position*), the
dominant lever-arm effect on a Doppler observation is kinematic: a rotating
body moves the antenna, so the range rate uses
v_antenna = v_body + ecef_R_body * (omega x leverArm)
with omega the measured body angular rate. The LOS/Sagnac terms use the
nominal receiver position (as in DopplerFactor), so the pose enters only
through attitude; with omega = 0 it reduces to DopplerFactor. An ecef_T_nav
overload supports a local nav-frame pose. Analytical Jacobians verified
against numerical derivatives (~1e-8) on the linked library.
Review fixes:
- Serialization: BOOST_SERIALIZATION_BASE_OBJECT_NVP used the qualified
`DopplerFactor::Base` / `ClockDriftFactor::Base`, producing an invalid XML
tag ("::") that made equalsXML throw. Use `Base` like the sibling factors.
Regression-caught by the new serialization tests below.
- Add serialization round-trip tests (obj/XML/binary) for DopplerFactor,
DopplerFactorArm (both overloads) and ClockDriftFactor.
- DopplerFactor::print() now also prints velSagnac_.
- Add DopplerFactorArm tests: reduction to base (omega=0), full model vs an
independent reference, ecef_T_nav Jacobians, and equals; plus the Python
binding in navigation.i.
Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
…ClockDriftFactor
Rework the GNSS Doppler factors to express the receiver clock drift as
(bias_k - bias_{k-1})/dt using the two adjacent clock-bias states already
estimated by the pseudorange/carrier factors, instead of a dedicated
clock-drift state:
- DopplerFactor keys change from [velocity, clockDrift] to
[velocity, clockBiasPrev, clockBiasCurr], with a new dt parameter
- DopplerFactorArm gains the same two-bias keying (4 keys total)
- ClockDriftFactor is removed: the Doppler measurement itself now
constrains the clock-bias evolution, so no extra between-epoch clock
factor or drift state is needed
This keeps the state vector identical to the pseudorange-only problem
(TDCP-like structure). Validated against the u-blox F9P sample data from
rtklibexplorer/rtklib-py (GPS L1, 30 epoch pairs): the joint two-epoch
solution matches the RTKLIB resdop-style estimation with an explicit
drift state to <0.3 mm/s in velocity and ~5e-13 s/s in drift.
Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01RUiPXeNrWr3cmpb2Lsa3HK
Co-Authored-By: Claude Fable 5 <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_01RUiPXeNrWr3cmpb2Lsa3HK
Doc/comment accuracy only (no behavior change): - Drop the inaccurate "time derivative of the Sagnac range term" wording; the implemented sagnac_rate is simply the earth-rotation correction to the range rate (the literal derivative of the geodist Sagnac term has the opposite sign, so the old phrasing was misleading). - Reword the base DopplerFactor note: the receiver position is a fixed input, not a state, so the line-of-sight is constant and enters no Jacobian (the old "second-order dependence ... is neglected" wording implied a state it does not have). - Document the new keying's usage: the two clock-bias keys must be distinct adjacent-epoch states (same key -> zero drift), the first epoch has no k-1 bias, and per-constellation biases share the common drift. - DopplerFactorArm::print() now also prints velSagnac_ and, when set, the ecef_T_nav transform (matching equals()/serialize()). - Add a DopplerFactorArm dt<=0 guard test. - Refresh the @Date. Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
…ck-drift-review-srk4qi # Conflicts: # gtsam/navigation/tests/testSerializationNavigation.cpp
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
2 participants
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Adds GNSS Doppler (range-rate) factors, complementing
PseudorangeFactor/CarrierPhaseFactor.DopplerFactor— keys[velocity, clock bias(k-1), clock bias(k)]:DopplerFactorArm— keys[pose, velocity, clock bias(k-1), clock bias(k)]:lever-arm variant using
v_antenna = v_body + ecef_R_body·(omega × leverArm),with an optional
ecef_T_navoverload; reduces toDopplerFactorwhen omega = 0.Both include the earth-rotation (Sagnac) rate correction and reuse the
Sagnac-corrected line-of-sight from
gnss::geodist. Analytical Jacobians(checked with
EXPECT_CORRECT_FACTOR_JACOBIANS), Python bindings, andobj/XML/binary serialization tests are included.