From da0b787fcc49506858fee69f64e98608c13918b1 Mon Sep 17 00:00:00 2001 From: inuex35 Date: Thu, 18 Jun 2026 09:31:39 +0900 Subject: [PATCH 1/8] navigation: add DopplerFactor and ClockDriftFactor 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) --- gtsam/navigation/DopplerFactor.cpp | 111 +++++++++++ gtsam/navigation/DopplerFactor.h | 192 +++++++++++++++++++ gtsam/navigation/navigation.i | 35 ++++ gtsam/navigation/tests/testDopplerFactor.cpp | 87 +++++++++ 4 files changed, 425 insertions(+) create mode 100644 gtsam/navigation/DopplerFactor.cpp create mode 100644 gtsam/navigation/DopplerFactor.h create mode 100644 gtsam/navigation/tests/testDopplerFactor.cpp diff --git a/gtsam/navigation/DopplerFactor.cpp b/gtsam/navigation/DopplerFactor.cpp new file mode 100644 index 0000000000..41a4f43382 --- /dev/null +++ b/gtsam/navigation/DopplerFactor.cpp @@ -0,0 +1,111 @@ +/** + * @file DopplerFactor.cpp + * @brief Implementation of the GNSS Doppler factor and constant-drift clock + * @date June 17, 2026 + **/ + +#include "DopplerFactor.h" + +namespace gtsam { + +using gnss::C_LIGHT; + +//*************************************************************************** +DopplerFactor::DopplerFactor(const Key velocityKey, const Key clockDriftKey, + const double measuredDoppler, + const double wavelength, + const Point3& satellitePosition, + const Point3& satelliteVelocity, + const Point3& receiverPosition, + const double satelliteClockDrift, + const SharedNoiseModel& model) + : Base(model, velocityKey, clockDriftKey), + measRangeRate_(-wavelength * measuredDoppler), + satVel_(satelliteVelocity), + satClkDrift_(satelliteClockDrift) { + // Line-of-sight unit vector (receiver -> satellite), Sagnac-aware geodist. + Point3 e; + gnss::geodist(satellitePosition, receiverPosition, e); + los_ = e; +} + +//*************************************************************************** +void DopplerFactor::print(const std::string& s, + const KeyFormatter& keyFormatter) const { + Base::print(s, keyFormatter); + gtsam::print(measRangeRate_, "measured range rate (m/s): "); + gtsam::print(Vector(satVel_), "sat velocity (ECEF m/s): "); + gtsam::print(Vector(los_), "line-of-sight (rcv->sat): "); + gtsam::print(satClkDrift_, "sat clock drift (s/s): "); +} + +//*************************************************************************** +bool DopplerFactor::equals(const NonlinearFactor& expected, double tol) const { + const This* e = dynamic_cast(&expected); + return e != nullptr && Base::equals(*e, tol) && + traits::Equals(measRangeRate_, e->measRangeRate_, tol) && + traits::Equals(satVel_, e->satVel_, tol) && + traits::Equals(los_, e->los_, tol) && + traits::Equals(satClkDrift_, e->satClkDrift_, tol); +} + +//*************************************************************************** +Vector DopplerFactor::evaluateError(const Vector3& velocity, + const double& clockDrift, + OptionalMatrixType Hvelocity, + OptionalMatrixType HclockDrift) const { + // range rate = e . (v_s - v_r) + c * (ddt_r - ddt_s) + const double rangeRate = + los_.dot(satVel_ - velocity) + C_LIGHT * (clockDrift - satClkDrift_); + const double error = rangeRate - measRangeRate_; + + if (Hvelocity) { + *Hvelocity = -los_.transpose(); // d/d v_r [ e . (v_s - v_r) ] = -e^T + } + if (HclockDrift) { + *HclockDrift = I_1x1 * C_LIGHT; + } + + return Vector1(error); +} + +//*************************************************************************** +ClockDriftFactor::ClockDriftFactor(const Key clockBiasPrevKey, + const Key clockBiasCurrKey, + const Key clockDriftKey, const double dt, + const SharedNoiseModel& model) + : Base(model, clockBiasPrevKey, clockBiasCurrKey, clockDriftKey), dt_(dt) {} + +//*************************************************************************** +void ClockDriftFactor::print(const std::string& s, + const KeyFormatter& keyFormatter) const { + Base::print(s, keyFormatter); + gtsam::print(dt_, "dt (s): "); +} + +//*************************************************************************** +bool ClockDriftFactor::equals(const NonlinearFactor& expected, + double tol) const { + const This* e = dynamic_cast(&expected); + return e != nullptr && Base::equals(*e, tol) && + traits::Equals(dt_, e->dt_, tol); +} + +//*************************************************************************** +Vector ClockDriftFactor::evaluateError(const double& clockBiasPrev, + const double& clockBiasCurr, + const double& clockDrift, + OptionalMatrixType HbiasPrev, + OptionalMatrixType HbiasCurr, + OptionalMatrixType Hdrift) const { + // Constant-velocity clock: bias(k) - bias(k-1) - drift*dt = 0 + const double error = clockBiasCurr - clockBiasPrev - clockDrift * dt_; + + if (HbiasPrev) *HbiasPrev = -I_1x1; + if (HbiasCurr) *HbiasCurr = I_1x1; + if (Hdrift) *Hdrift = -I_1x1 * dt_; + + return Vector1(error); +} + +} // namespace gtsam diff --git a/gtsam/navigation/DopplerFactor.h b/gtsam/navigation/DopplerFactor.h new file mode 100644 index 0000000000..b4e00b3525 --- /dev/null +++ b/gtsam/navigation/DopplerFactor.h @@ -0,0 +1,192 @@ +/** + * @file DopplerFactor.h + * @brief Header file for the GNSS Doppler (range-rate) factor + * @date June 17, 2026 + **/ +#pragma once + +#include +#include +#include +#include +#include + +#include + +namespace gtsam { + +/** + * GNSS Doppler (range-rate) factor. + * + * Relates a measured Doppler observation to the receiver velocity and clock + * drift, reusing the line-of-sight unit vector from gnss::geodist: + * + * error = e . (v_s - v_r) + c * (ddt_r - ddt_s) - (-lambda * Doppler) + * + * where + * - e is the unit line-of-sight vector from receiver to satellite, + * - v_s, v_r are the satellite and receiver ECEF velocities (m/s), + * - ddt_r is the receiver clock drift (s/s) -- the estimated state, + * - ddt_s is the satellite clock drift (s/s), + * - lambda * Doppler is the measured range rate (m/s); a positive Doppler + * (approaching satellite) corresponds to a decreasing range, hence the + * leading minus sign. + * + * The satellite velocity and the line-of-sight are held constant per factor; + * their (second-order) dependence on the receiver position is neglected, which + * is standard for Doppler velocity estimation. Doppler provides receiver + * velocity and clock-drift observability that is robust through carrier-phase + * cycle slips and re-convergence. + * + * Keys: [velocity (Vector3, m/s), receiver clock drift (double, s/s)]. + * + * @ingroup navigation + */ +class GTSAM_EXPORT DopplerFactor : public NoiseModelFactorN { + private: + typedef NoiseModelFactorN Base; + + double measRangeRate_ = 0.0; ///< Measured range rate = -lambda*D [m/s]. + Point3 satVel_{0, 0, 0}; ///< Satellite ECEF velocity [m/s]. + Point3 los_{0, 0, 0}; ///< Unit LOS, receiver -> satellite. + double satClkDrift_ = 0.0; ///< Satellite clock drift [s/s]. + + public: + using Base::evaluateError; + typedef std::shared_ptr shared_ptr; + typedef DopplerFactor This; + + /** default constructor - only use for serialization */ + DopplerFactor() = default; + virtual ~DopplerFactor() = default; + + /** + * @param velocityKey Receiver ECEF velocity node (Vector3, m/s). + * @param clockDriftKey Receiver clock drift node (double, s/s). + * @param measuredDoppler Measured Doppler [Hz]. + * @param wavelength Carrier wavelength [m/cycle]. + * @param satellitePosition Satellite ECEF position [m] (for the LOS). + * @param satelliteVelocity Satellite ECEF velocity [m/s]. + * @param receiverPosition Receiver ECEF position [m] (for the LOS). + * @param satelliteClockDrift Satellite clock drift [s/s]. + * @param model 1-D range-rate noise model. + */ + DopplerFactor(Key velocityKey, Key clockDriftKey, double measuredDoppler, + double wavelength, const Point3& satellitePosition, + const Point3& satelliteVelocity, const Point3& receiverPosition, + double satelliteClockDrift = 0.0, + const SharedNoiseModel& model = noiseModel::Unit::Create(1)); + + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const override; + bool equals(const NonlinearFactor& expected, + double tol = 1e-9) const override; + + Vector evaluateError(const Vector3& velocity, const double& clockDrift, + OptionalMatrixType Hvelocity, + OptionalMatrixType HclockDrift) const override; + + /// Measured range rate (= -lambda * Doppler) [m/s]. + inline double measuredRangeRate() const { return measRangeRate_; } + /// Unit line-of-sight vector (receiver -> satellite). + inline const Point3& lineOfSight() const { return los_; } + + private: +#if GTSAM_ENABLE_BOOST_SERIALIZATION + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(DopplerFactor::Base); + ar& BOOST_SERIALIZATION_NVP(measRangeRate_); + ar& BOOST_SERIALIZATION_NVP(satVel_); + ar& BOOST_SERIALIZATION_NVP(los_); + ar& BOOST_SERIALIZATION_NVP(satClkDrift_); + } +#endif +}; + +/// traits +template <> +struct traits : public Testable {}; + +/** + * Constant-drift receiver clock factor. + * + * Links two consecutive receiver clock biases through a (constant) clock drift, + * i.e. the constant-velocity clock model -- the scalar analog of how the IMU + * factor links position and velocity: + * + * error = bias(k) - bias(k-1) - drift * dt + * + * The drift state is shared by the DopplerFactor (which observes it) and by this + * factor (which integrates it into the clock bias used by the pseudorange / + * carrier-phase factors), so Doppler velocity information propagates into the + * clock bias. Bias is in seconds, drift in seconds/second, dt in seconds. + * + * Keys: [clock bias prev (double), clock bias curr (double), drift (double)]. + * + * @ingroup navigation + */ +class GTSAM_EXPORT ClockDriftFactor + : public NoiseModelFactorN { + private: + typedef NoiseModelFactorN Base; + double dt_ = 0.0; ///< Time step between the two clock-bias epochs [s]. + + public: + using Base::evaluateError; + typedef std::shared_ptr shared_ptr; + typedef ClockDriftFactor This; + + ClockDriftFactor() = default; + virtual ~ClockDriftFactor() = default; + + /** + * @param clockBiasPrevKey Receiver clock bias at epoch k-1 [s]. + * @param clockBiasCurrKey Receiver clock bias at epoch k [s]. + * @param clockDriftKey Receiver clock drift [s/s]. + * @param dt Time step (t_k - t_{k-1}) [s]. + * @param model 1-D noise model (clock process noise). + */ + ClockDriftFactor(Key clockBiasPrevKey, Key clockBiasCurrKey, + Key clockDriftKey, double dt, + const SharedNoiseModel& model = noiseModel::Unit::Create(1)); + + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const override; + bool equals(const NonlinearFactor& expected, + double tol = 1e-9) const override; + + Vector evaluateError(const double& clockBiasPrev, const double& clockBiasCurr, + const double& clockDrift, OptionalMatrixType HbiasPrev, + OptionalMatrixType HbiasCurr, + OptionalMatrixType Hdrift) const override; + + inline double dt() const { return dt_; } + + private: +#if GTSAM_ENABLE_BOOST_SERIALIZATION + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(ClockDriftFactor::Base); + ar& BOOST_SERIALIZATION_NVP(dt_); + } +#endif +}; + +/// traits +template <> +struct traits : public Testable {}; + +} // namespace gtsam diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 6e6de476b2..46bdfd35fd 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -728,6 +728,41 @@ virtual class DoubleDifferenceCarrierPhaseFactorArm : gtsam::NonlinearFactor { void serialize() const; }; +#include +virtual class DopplerFactor : gtsam::NonlinearFactor { + DopplerFactor(gtsam::Key velocityKey, gtsam::Key clockDriftKey, + double measuredDoppler, double wavelength, + const gtsam::Point3& satellitePosition, + const gtsam::Point3& satelliteVelocity, + const gtsam::Point3& receiverPosition, + double satelliteClockDrift, + const gtsam::noiseModel::Base* model); + + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::NonlinearFactor& expected, double tol) const; + gtsam::Vector evaluateError(const gtsam::Vector3& velocity, + const double& clockDrift) const; + double measuredRangeRate() const; + const gtsam::Point3& lineOfSight() const; + void serialize() const; +}; + +virtual class ClockDriftFactor : gtsam::NonlinearFactor { + ClockDriftFactor(gtsam::Key clockBiasPrevKey, gtsam::Key clockBiasCurrKey, + gtsam::Key clockDriftKey, double dt, + const gtsam::noiseModel::Base* model); + + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::NonlinearFactor& expected, double tol) const; + gtsam::Vector evaluateError(const double& clockBiasPrev, + const double& clockBiasCurr, + const double& clockDrift) const; + double dt() const; + void serialize() const; +}; + #include virtual class BarometricFactor : gtsam::NonlinearFactor { BarometricFactor(); diff --git a/gtsam/navigation/tests/testDopplerFactor.cpp b/gtsam/navigation/tests/testDopplerFactor.cpp new file mode 100644 index 0000000000..6cbecda1a6 --- /dev/null +++ b/gtsam/navigation/tests/testDopplerFactor.cpp @@ -0,0 +1,87 @@ +/** + * @file testDopplerFactor.cpp + * @brief Unit tests for DopplerFactor and ClockDriftFactor + * @date June 17, 2026 + */ + +#include +#include +#include +#include +#include + +#include + +using namespace gtsam; +using namespace gtsam::gnss_test; + +// ************************************************************************* +TEST(TestDopplerFactor, Model) { + const Point3 satVel(-1200.0, 2400.0, 800.0); // sat ECEF velocity [m/s] + const Point3 rcvVel(0.3, -0.1, 0.05); // rover velocity [m/s] + const double measDoppler = -1500.0; // [Hz] + const double satClkDrift = 1.2e-9; // [s/s] + const double rcvClkDrift = 4.5e-9; // [s/s] + + const auto factor = DopplerFactor( + Key(0), Key(1), measDoppler, kLambdaL1, sample::kSatPos, satVel, + sample::kReceiverPos, satClkDrift); + + const double error = + factor.evaluateError((Vector3)rcvVel, rcvClkDrift)[0]; + + // Reference: range rate via Sagnac-aware LOS minus measured range rate. + Point3 e; + gnss::geodist(sample::kSatPos, sample::kReceiverPos, e); + const double rangeRate = + e.dot(satVel - rcvVel) + kCLight * (rcvClkDrift - satClkDrift); + const double expected = rangeRate - (-kLambdaL1 * measDoppler); + EXPECT_DOUBLES_EQUAL(expected, error, 1e-6); + + Values values; + values.insert(Key(0), (Vector3)rcvVel); + values.insert(Key(1), rcvClkDrift); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-3, 1e-5); +} + +// ************************************************************************* +TEST(TestDopplerFactor, equals) { + const Point3 satVel(100, 200, 300); + const auto f1 = DopplerFactor(0, 1, 10.0, kLambdaL1, sample::kSatPos, satVel, + sample::kReceiverPos, 0.0); + const auto f2 = DopplerFactor(0, 1, 10.0, kLambdaL1, sample::kSatPos, satVel, + sample::kReceiverPos, 0.0); + const auto f3 = DopplerFactor(0, 1, 99.0, kLambdaL1, sample::kSatPos, satVel, + sample::kReceiverPos, 0.0); + CHECK(f1.equals(f2)); + CHECK(!f1.equals(f3)); + f1.print("doppler "); +} + +// ************************************************************************* +TEST(TestClockDriftFactor, Model) { + const double dt = 0.2; + const double biasPrev = 1.0e-6, drift = 4.5e-9; + const double biasCurr = biasPrev + drift * dt; // zero-error point + + const auto factor = ClockDriftFactor(Key(0), Key(1), Key(2), dt); + const double error = factor.evaluateError(biasPrev, biasCurr, drift)[0]; + EXPECT_DOUBLES_EQUAL(0.0, error, 1e-15); + + // Off the constraint: + const double error2 = factor.evaluateError(biasPrev, biasCurr + 1e-7, drift)[0]; + EXPECT_DOUBLES_EQUAL(1e-7, error2, 1e-15); + + Values values; + values.insert(Key(0), biasPrev); + values.insert(Key(1), biasCurr); + values.insert(Key(2), drift); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-3, 1e-6); +} + +// ************************************************************************* +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +// ************************************************************************* From 299bf3c256493f15270bdb49e8a05b561f505c8a Mon Sep 17 00:00:00 2001 From: inuex35 Date: Thu, 2 Jul 2026 13:28:39 +0900 Subject: [PATCH 2/8] navigation: add earth-rotation (Sagnac) rate term to DopplerFactor 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) --- gtsam/navigation/DopplerFactor.cpp | 24 ++++++++++++++++---- gtsam/navigation/DopplerFactor.h | 18 +++++++++++---- gtsam/navigation/tests/testDopplerFactor.cpp | 13 ++++++++--- 3 files changed, 43 insertions(+), 12 deletions(-) diff --git a/gtsam/navigation/DopplerFactor.cpp b/gtsam/navigation/DopplerFactor.cpp index 41a4f43382..172508b257 100644 --- a/gtsam/navigation/DopplerFactor.cpp +++ b/gtsam/navigation/DopplerFactor.cpp @@ -9,6 +9,7 @@ namespace gtsam { using gnss::C_LIGHT; +using gnss::OMGE; //*************************************************************************** DopplerFactor::DopplerFactor(const Key velocityKey, const Key clockDriftKey, @@ -27,6 +28,14 @@ DopplerFactor::DopplerFactor(const Key velocityKey, const Key clockDriftKey, Point3 e; gnss::geodist(satellitePosition, receiverPosition, e); los_ = e; + + // Earth-rotation (Sagnac) rate term, matching RTKLIB resdop(): + // (OMGE/c) * (v_s.y*r_r.x + r_s.y*v_r.x - v_s.x*r_r.y - r_s.x*v_r.y) + // Split into the v_r-independent offset and the linear coefficient on v_r. + const double k = OMGE / C_LIGHT; + sagnacOffset_ = k * (satelliteVelocity.y() * receiverPosition.x() - + satelliteVelocity.x() * receiverPosition.y()); + velSagnac_ = Point3(k * satellitePosition.y(), -k * satellitePosition.x(), 0.0); } //*************************************************************************** @@ -37,6 +46,7 @@ void DopplerFactor::print(const std::string& s, gtsam::print(Vector(satVel_), "sat velocity (ECEF m/s): "); gtsam::print(Vector(los_), "line-of-sight (rcv->sat): "); gtsam::print(satClkDrift_, "sat clock drift (s/s): "); + gtsam::print(sagnacOffset_, "Sagnac rate offset (m/s): "); } //*************************************************************************** @@ -46,7 +56,9 @@ bool DopplerFactor::equals(const NonlinearFactor& expected, double tol) const { traits::Equals(measRangeRate_, e->measRangeRate_, tol) && traits::Equals(satVel_, e->satVel_, tol) && traits::Equals(los_, e->los_, tol) && - traits::Equals(satClkDrift_, e->satClkDrift_, tol); + traits::Equals(satClkDrift_, e->satClkDrift_, tol) && + traits::Equals(velSagnac_, e->velSagnac_, tol) && + traits::Equals(sagnacOffset_, e->sagnacOffset_, tol); } //*************************************************************************** @@ -54,13 +66,15 @@ Vector DopplerFactor::evaluateError(const Vector3& velocity, const double& clockDrift, OptionalMatrixType Hvelocity, OptionalMatrixType HclockDrift) const { - // range rate = e . (v_s - v_r) + c * (ddt_r - ddt_s) - const double rangeRate = - los_.dot(satVel_ - velocity) + C_LIGHT * (clockDrift - satClkDrift_); + // range rate = e . (v_s - v_r) + c * (ddt_r - ddt_s) + sagnac_rate + const double rangeRate = los_.dot(satVel_ - velocity) + + C_LIGHT * (clockDrift - satClkDrift_) + + sagnacOffset_ + velSagnac_.dot(velocity); const double error = rangeRate - measRangeRate_; if (Hvelocity) { - *Hvelocity = -los_.transpose(); // d/d v_r [ e . (v_s - v_r) ] = -e^T + // d/d v_r [ e . (v_s - v_r) + velSagnac . v_r ] = (velSagnac - e)^T + *Hvelocity = (velSagnac_ - los_).transpose(); } if (HclockDrift) { *HclockDrift = I_1x1 * C_LIGHT; diff --git a/gtsam/navigation/DopplerFactor.h b/gtsam/navigation/DopplerFactor.h index b4e00b3525..d92b985c89 100644 --- a/gtsam/navigation/DopplerFactor.h +++ b/gtsam/navigation/DopplerFactor.h @@ -21,20 +21,26 @@ namespace gtsam { * Relates a measured Doppler observation to the receiver velocity and clock * drift, reusing the line-of-sight unit vector from gnss::geodist: * - * error = e . (v_s - v_r) + c * (ddt_r - ddt_s) - (-lambda * Doppler) + * error = e . (v_s - v_r) + c * (ddt_r - ddt_s) + sagnac_rate + * - (-lambda * Doppler) * * where * - e is the unit line-of-sight vector from receiver to satellite, * - v_s, v_r are the satellite and receiver ECEF velocities (m/s), * - ddt_r is the receiver clock drift (s/s) -- the estimated state, * - ddt_s is the satellite clock drift (s/s), + * - sagnac_rate = (OMGE/c) * (v_s.y*r_r.x + r_s.y*v_r.x + * - v_s.x*r_r.y - r_s.x*v_r.y) + * is the earth-rotation (Sagnac) rate correction -- the time derivative of + * the Sagnac range term, matching RTKLIB's resdop(), * - lambda * Doppler is the measured range rate (m/s); a positive Doppler * (approaching satellite) corresponds to a decreasing range, hence the * leading minus sign. * - * The satellite velocity and the line-of-sight are held constant per factor; - * their (second-order) dependence on the receiver position is neglected, which - * is standard for Doppler velocity estimation. Doppler provides receiver + * The satellite position/velocity and the line-of-sight are held constant per + * factor; only the (second-order) dependence of the line-of-sight on the + * receiver position is neglected, which is standard for Doppler velocity + * estimation. Doppler provides receiver * velocity and clock-drift observability that is robust through carrier-phase * cycle slips and re-convergence. * @@ -50,6 +56,8 @@ class GTSAM_EXPORT DopplerFactor : public NoiseModelFactorN { Point3 satVel_{0, 0, 0}; ///< Satellite ECEF velocity [m/s]. Point3 los_{0, 0, 0}; ///< Unit LOS, receiver -> satellite. double satClkDrift_ = 0.0; ///< Satellite clock drift [s/s]. + Point3 velSagnac_{0, 0, 0}; ///< Sagnac rate coeff, d(rate)/d(v_r). + double sagnacOffset_ = 0.0; ///< v_r-independent Sagnac rate term [m/s]. public: using Base::evaluateError; @@ -106,6 +114,8 @@ class GTSAM_EXPORT DopplerFactor : public NoiseModelFactorN { ar& BOOST_SERIALIZATION_NVP(satVel_); ar& BOOST_SERIALIZATION_NVP(los_); ar& BOOST_SERIALIZATION_NVP(satClkDrift_); + ar& BOOST_SERIALIZATION_NVP(velSagnac_); + ar& BOOST_SERIALIZATION_NVP(sagnacOffset_); } #endif }; diff --git a/gtsam/navigation/tests/testDopplerFactor.cpp b/gtsam/navigation/tests/testDopplerFactor.cpp index 6cbecda1a6..69f0492eee 100644 --- a/gtsam/navigation/tests/testDopplerFactor.cpp +++ b/gtsam/navigation/tests/testDopplerFactor.cpp @@ -30,11 +30,18 @@ TEST(TestDopplerFactor, Model) { const double error = factor.evaluateError((Vector3)rcvVel, rcvClkDrift)[0]; - // Reference: range rate via Sagnac-aware LOS minus measured range rate. + // Reference: range rate via Sagnac-aware LOS, plus the earth-rotation + // (Sagnac) rate term, minus the measured range rate. Point3 e; gnss::geodist(sample::kSatPos, sample::kReceiverPos, e); - const double rangeRate = - e.dot(satVel - rcvVel) + kCLight * (rcvClkDrift - satClkDrift); + const double kSag = gnss::OMGE / kCLight; + const double sagnacRate = + kSag * (satVel.y() * sample::kReceiverPos.x() + + sample::kSatPos.y() * rcvVel.x() - + satVel.x() * sample::kReceiverPos.y() - + sample::kSatPos.x() * rcvVel.y()); + const double rangeRate = e.dot(satVel - rcvVel) + + kCLight * (rcvClkDrift - satClkDrift) + sagnacRate; const double expected = rangeRate - (-kLambdaL1 * measDoppler); EXPECT_DOUBLES_EQUAL(expected, error, 1e-6); From 8ff87f55ecd15dae985acf5c829c818fec2b7b56 Mon Sep 17 00:00:00 2001 From: inuex35 Date: Thu, 2 Jul 2026 20:55:51 +0900 Subject: [PATCH 3/8] navigation: add DopplerFactorArm (lever-arm) + fix serialization/print 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) --- gtsam/navigation/DopplerFactor.cpp | 132 ++++++++++++++++ gtsam/navigation/DopplerFactor.h | 142 +++++++++++++++++- gtsam/navigation/navigation.i | 32 ++++ gtsam/navigation/tests/testDopplerFactor.cpp | 96 ++++++++++++ .../tests/testSerializationNavigation.cpp | 37 +++++ 5 files changed, 437 insertions(+), 2 deletions(-) diff --git a/gtsam/navigation/DopplerFactor.cpp b/gtsam/navigation/DopplerFactor.cpp index 172508b257..451b017ef3 100644 --- a/gtsam/navigation/DopplerFactor.cpp +++ b/gtsam/navigation/DopplerFactor.cpp @@ -46,6 +46,7 @@ void DopplerFactor::print(const std::string& s, gtsam::print(Vector(satVel_), "sat velocity (ECEF m/s): "); gtsam::print(Vector(los_), "line-of-sight (rcv->sat): "); gtsam::print(satClkDrift_, "sat clock drift (s/s): "); + gtsam::print(Vector(velSagnac_), "Sagnac rate coeff (1/s): "); gtsam::print(sagnacOffset_, "Sagnac rate offset (m/s): "); } @@ -83,6 +84,137 @@ Vector DopplerFactor::evaluateError(const Vector3& velocity, return Vector1(error); } +//*************************************************************************** +// DopplerFactorArm +//*************************************************************************** +namespace { +// Shared geometry precompute for the two DopplerFactorArm constructors: the +// LOS unit vector and the (nominal-position) Sagnac rate terms, identical to +// DopplerFactor, plus the body-frame lever-arm velocity omega x b. +void initDopplerArmGeometry(const Point3& satellitePosition, + const Point3& receiverPosition, + const Point3& satelliteVelocity, + const Point3& angularVelocity, + const Point3& leverArm, Point3& los, Point3& velSagnac, + double& sagnacOffset, Point3& leverVel) { + gnss::geodist(satellitePosition, receiverPosition, los); + const double k = OMGE / C_LIGHT; + sagnacOffset = k * (satelliteVelocity.y() * receiverPosition.x() - + satelliteVelocity.x() * receiverPosition.y()); + velSagnac = Point3(k * satellitePosition.y(), -k * satellitePosition.x(), 0.0); + leverVel = angularVelocity.cross(leverArm); +} +} // namespace + +DopplerFactorArm::DopplerFactorArm( + const Key poseKey, const Key velocityKey, const Key clockDriftKey, + const double measuredDoppler, const double wavelength, + const Point3& satellitePosition, const Point3& satelliteVelocity, + const Point3& receiverPosition, const Point3& leverArm, + const Point3& angularVelocity, const double satelliteClockDrift, + const SharedNoiseModel& model) + : Base(model, poseKey, velocityKey, clockDriftKey), + measRangeRate_(-wavelength * measuredDoppler), + satVel_(satelliteVelocity), + satClkDrift_(satelliteClockDrift), + arm_(leverArm) { + initDopplerArmGeometry(satellitePosition, receiverPosition, satelliteVelocity, + angularVelocity, leverArm, los_, velSagnac_, + sagnacOffset_, leverVel_); +} + +DopplerFactorArm::DopplerFactorArm( + const Key poseKey, const Key velocityKey, const Key clockDriftKey, + const double measuredDoppler, const double wavelength, + const Point3& satellitePosition, const Point3& satelliteVelocity, + const Point3& receiverPosition, const Point3& leverArm, + const Pose3& ecef_T_nav, const Point3& angularVelocity, + const double satelliteClockDrift, const SharedNoiseModel& model) + : Base(model, poseKey, velocityKey, clockDriftKey), + measRangeRate_(-wavelength * measuredDoppler), + satVel_(satelliteVelocity), + satClkDrift_(satelliteClockDrift), + arm_(leverArm, ecef_T_nav) { + initDopplerArmGeometry(satellitePosition, receiverPosition, satelliteVelocity, + angularVelocity, leverArm, los_, velSagnac_, + sagnacOffset_, leverVel_); +} + +//*************************************************************************** +void DopplerFactorArm::print(const std::string& s, + const KeyFormatter& keyFormatter) const { + Base::print(s, keyFormatter); + gtsam::print(measRangeRate_, "measured range rate (m/s): "); + gtsam::print(Vector(satVel_), "sat velocity (ECEF m/s): "); + gtsam::print(Vector(los_), "line-of-sight (rcv->sat): "); + gtsam::print(satClkDrift_, "sat clock drift (s/s): "); + gtsam::print(sagnacOffset_, "Sagnac rate offset (m/s): "); + gtsam::print(Vector(arm_.b), "lever arm (body m): "); + gtsam::print(Vector(leverVel_), "lever velocity omega x b (m/s): "); +} + +//*************************************************************************** +bool DopplerFactorArm::equals(const NonlinearFactor& expected, + double tol) const { + const This* e = dynamic_cast(&expected); + return e != nullptr && Base::equals(*e, tol) && + traits::Equals(measRangeRate_, e->measRangeRate_, tol) && + traits::Equals(satVel_, e->satVel_, tol) && + traits::Equals(los_, e->los_, tol) && + traits::Equals(satClkDrift_, e->satClkDrift_, tol) && + traits::Equals(velSagnac_, e->velSagnac_, tol) && + traits::Equals(sagnacOffset_, e->sagnacOffset_, tol) && + arm_.equals(e->arm_, tol) && + traits::Equals(leverVel_, e->leverVel_, tol); +} + +//*************************************************************************** +Vector DopplerFactorArm::evaluateError(const Pose3& pose, + const Vector3& velocity, + const double& clockDrift, + OptionalMatrixType Hpose, + OptionalMatrixType Hvelocity, + OptionalMatrixType HclockDrift) const { + // Lever-arm velocity in ECEF: v_lever = ecef_R_body * (omega x leverArm). + // Hrot is d(v_lever)/d(rotation tangent) [3x3]. + Matrix3 Hrot; + Point3 leverVelEcef; + if (arm_.ecef_T_nav) { + Matrix3 Hinner; + const Point3 vNav = pose.rotation().rotate(leverVel_, Hinner); + const Matrix3 Recn = arm_.ecef_T_nav->rotation().matrix(); + leverVelEcef = arm_.ecef_T_nav->rotation().rotate(vNav); + Hrot = Recn * Hinner; + } else { + leverVelEcef = pose.rotation().rotate(leverVel_, Hrot); + } + const Vector3 vAnt = velocity + Vector3(leverVelEcef); + + // Effective range-rate coefficient on the antenna velocity: (velSagnac - e). + const Vector3 g = Vector3(velSagnac_) - Vector3(los_); + const double rangeRate = los_.dot(satVel_) + + C_LIGHT * (clockDrift - satClkDrift_) + + sagnacOffset_ + g.dot(vAnt); + const double error = rangeRate - measRangeRate_; + + if (Hpose) { + // Pose enters only through v_ant = ... + R*(omega x b); the LOS/Sagnac use + // the fixed nominal position, so the translation block is zero. Pose3 + // tangent order is [rotation(3), translation(3)]. + Matrix16 H = Matrix16::Zero(); + H.block<1, 3>(0, 0) = g.transpose() * Hrot; + *Hpose = H; + } + if (Hvelocity) { + *Hvelocity = g.transpose(); + } + if (HclockDrift) { + *HclockDrift = I_1x1 * C_LIGHT; + } + + return Vector1(error); +} + //*************************************************************************** ClockDriftFactor::ClockDriftFactor(const Key clockBiasPrevKey, const Key clockBiasCurrKey, diff --git a/gtsam/navigation/DopplerFactor.h b/gtsam/navigation/DopplerFactor.h index d92b985c89..70d2be4af7 100644 --- a/gtsam/navigation/DopplerFactor.h +++ b/gtsam/navigation/DopplerFactor.h @@ -109,7 +109,7 @@ class GTSAM_EXPORT DopplerFactor : public NoiseModelFactorN { friend class boost::serialization::access; template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(DopplerFactor::Base); + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar& BOOST_SERIALIZATION_NVP(measRangeRate_); ar& BOOST_SERIALIZATION_NVP(satVel_); ar& BOOST_SERIALIZATION_NVP(los_); @@ -124,6 +124,144 @@ class GTSAM_EXPORT DopplerFactor : public NoiseModelFactorN { template <> struct traits : public Testable {}; +/** + * GNSS Doppler factor with lever-arm correction. + * + * Like DopplerFactor, but keys on a body Pose3 so the antenna's lever arm can + * be accounted for. Unlike the pseudorange/carrier lever-arm factors (which + * correct the antenna *position*), the dominant lever-arm effect on a Doppler + * (range-rate) observation is *kinematic*: when the body rotates at angular + * rate omega, the antenna moves relative to the body origin, so + * + * v_antenna = v_body + ecef_R_body * (omega x leverArm) + * + * where omega is the (measured) body-frame angular velocity and leverArm is the + * body-frame antenna offset. The range-rate error then uses v_antenna: + * + * error = e . (v_s - v_antenna) + c*(ddt_r - ddt_s) + sagnac_rate + * - (-lambda*Doppler) + * + * The line-of-sight e and the Sagnac terms are evaluated at the provided + * (nominal) receiver position, exactly as in DopplerFactor -- the second-order + * dependence of the LOS on the pose translation is neglected (standard for + * Doppler velocity estimation), so the pose enters only through its attitude. + * With omega = 0 the factor reduces to DopplerFactor evaluated at `velocity`. + * + * When the optional ecef_T_nav transform is provided, the pose key is a local + * navigation-frame pose (e.g. ENU) and ecef_R_body = ecef_R_nav * nav_R_body; + * `velocity` is still the receiver ECEF velocity. + * + * Keys: [pose (Pose3), velocity (Vector3, ECEF m/s), clock drift (double, s/s)]. + * + * @ingroup navigation + */ +class GTSAM_EXPORT DopplerFactorArm + : public NoiseModelFactorN { + private: + typedef NoiseModelFactorN Base; + + double measRangeRate_ = 0.0; ///< Measured range rate = -lambda*D [m/s]. + Point3 satVel_{0, 0, 0}; ///< Satellite ECEF velocity [m/s]. + Point3 los_{0, 0, 0}; ///< Unit LOS, receiver -> satellite. + double satClkDrift_ = 0.0; ///< Satellite clock drift [s/s]. + Point3 velSagnac_{0, 0, 0}; ///< Sagnac rate coeff, d(rate)/d(v_ant). + double sagnacOffset_ = 0.0; ///< v_ant-independent Sagnac rate term [m/s]. + gnss::LeverArm arm_; ///< Lever arm (body frame) + optional ecef_T_nav. + Point3 leverVel_{0, 0, 0}; ///< omega x leverArm (body frame) [m/s]. + + public: + using Base::evaluateError; + typedef std::shared_ptr shared_ptr; + typedef DopplerFactorArm This; + + /** default constructor - only use for serialization */ + DopplerFactorArm() = default; + virtual ~DopplerFactorArm() = default; + + /** + * Construct a DopplerFactorArm with an ECEF pose key. + * + * @param poseKey Receiver body Pose3 (ECEF) node. + * @param velocityKey Receiver ECEF velocity node (Vector3, m/s). + * @param clockDriftKey Receiver clock drift node (double, s/s). + * @param measuredDoppler Measured Doppler [Hz]. + * @param wavelength Carrier wavelength [m/cycle]. + * @param satellitePosition Satellite ECEF position [m] (for the LOS). + * @param satelliteVelocity Satellite ECEF velocity [m/s]. + * @param receiverPosition Nominal receiver ECEF position [m] (for the LOS). + * @param leverArm Antenna lever arm in the body frame [m]. + * @param angularVelocity Body-frame angular velocity omega [rad/s]. + * @param satelliteClockDrift Satellite clock drift [s/s]. + * @param model 1-D range-rate noise model. + */ + DopplerFactorArm(Key poseKey, Key velocityKey, Key clockDriftKey, + double measuredDoppler, double wavelength, + const Point3& satellitePosition, + const Point3& satelliteVelocity, + const Point3& receiverPosition, const Point3& leverArm, + const Point3& angularVelocity, + double satelliteClockDrift = 0.0, + const SharedNoiseModel& model = noiseModel::Unit::Create(1)); + + /// Construct with a local nav-frame pose key + ecef_T_nav. + DopplerFactorArm(Key poseKey, Key velocityKey, Key clockDriftKey, + double measuredDoppler, double wavelength, + const Point3& satellitePosition, + const Point3& satelliteVelocity, + const Point3& receiverPosition, const Point3& leverArm, + const Pose3& ecef_T_nav, const Point3& angularVelocity, + double satelliteClockDrift = 0.0, + const SharedNoiseModel& model = noiseModel::Unit::Create(1)); + + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const override; + bool equals(const NonlinearFactor& expected, + double tol = 1e-9) const override; + + Vector evaluateError(const Pose3& pose, const Vector3& velocity, + const double& clockDrift, OptionalMatrixType Hpose, + OptionalMatrixType Hvelocity, + OptionalMatrixType HclockDrift) const override; + + /// Measured range rate (= -lambda * Doppler) [m/s]. + inline double measuredRangeRate() const { return measRangeRate_; } + /// Unit line-of-sight vector (receiver -> satellite). + inline const Point3& lineOfSight() const { return los_; } + /// Lever arm in the body frame [m]. + inline const Point3& leverArm() const { return arm_.b; } + /// Optional ECEF-from-nav transform. + inline const std::optional& ecefTnav() const { + return arm_.ecef_T_nav; + } + + private: +#if GTSAM_ENABLE_BOOST_SERIALIZATION + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(measRangeRate_); + ar& BOOST_SERIALIZATION_NVP(satVel_); + ar& BOOST_SERIALIZATION_NVP(los_); + ar& BOOST_SERIALIZATION_NVP(satClkDrift_); + ar& BOOST_SERIALIZATION_NVP(velSagnac_); + ar& BOOST_SERIALIZATION_NVP(sagnacOffset_); + ar& boost::serialization::make_nvp("bL_", arm_.b); + ar& boost::serialization::make_nvp("ecef_T_nav_", arm_.ecef_T_nav); + ar& BOOST_SERIALIZATION_NVP(leverVel_); + } +#endif +}; + +/// traits +template <> +struct traits : public Testable {}; + /** * Constant-drift receiver clock factor. * @@ -189,7 +327,7 @@ class GTSAM_EXPORT ClockDriftFactor friend class boost::serialization::access; template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(ClockDriftFactor::Base); + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar& BOOST_SERIALIZATION_NVP(dt_); } #endif diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 46bdfd35fd..715b01f0f6 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -748,6 +748,38 @@ virtual class DopplerFactor : gtsam::NonlinearFactor { void serialize() const; }; +virtual class DopplerFactorArm : gtsam::NonlinearFactor { + DopplerFactorArm(gtsam::Key poseKey, gtsam::Key velocityKey, + gtsam::Key clockDriftKey, double measuredDoppler, + double wavelength, const gtsam::Point3& satellitePosition, + const gtsam::Point3& satelliteVelocity, + const gtsam::Point3& receiverPosition, + const gtsam::Point3& leverArm, + const gtsam::Point3& angularVelocity, + double satelliteClockDrift, + const gtsam::noiseModel::Base* model); + DopplerFactorArm(gtsam::Key poseKey, gtsam::Key velocityKey, + gtsam::Key clockDriftKey, double measuredDoppler, + double wavelength, const gtsam::Point3& satellitePosition, + const gtsam::Point3& satelliteVelocity, + const gtsam::Point3& receiverPosition, + const gtsam::Point3& leverArm, const gtsam::Pose3& ecef_T_nav, + const gtsam::Point3& angularVelocity, + double satelliteClockDrift, + const gtsam::noiseModel::Base* model); + + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::NonlinearFactor& expected, double tol) const; + gtsam::Vector evaluateError(const gtsam::Pose3& pose, + const gtsam::Vector3& velocity, + const double& clockDrift) const; + double measuredRangeRate() const; + const gtsam::Point3& lineOfSight() const; + const gtsam::Point3& leverArm() const; + void serialize() const; +}; + virtual class ClockDriftFactor : gtsam::NonlinearFactor { ClockDriftFactor(gtsam::Key clockBiasPrevKey, gtsam::Key clockBiasCurrKey, gtsam::Key clockDriftKey, double dt, diff --git a/gtsam/navigation/tests/testDopplerFactor.cpp b/gtsam/navigation/tests/testDopplerFactor.cpp index 69f0492eee..c2146b965f 100644 --- a/gtsam/navigation/tests/testDopplerFactor.cpp +++ b/gtsam/navigation/tests/testDopplerFactor.cpp @@ -86,6 +86,102 @@ TEST(TestClockDriftFactor, Model) { EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-3, 1e-6); } +// ************************************************************************* +// DopplerFactorArm: with omega = 0 it must reduce to DopplerFactor at the same +// velocity, independent of the pose attitude (lever velocity omega x b = 0). +TEST(TestDopplerFactorArm, ReducesToBaseWhenNoRotationRate) { + const Point3 satVel(-1200.0, 2400.0, 800.0); + const Vector3 rcvVel(0.3, -0.1, 0.05); + const double measDoppler = -1500.0, satClkDrift = 1.2e-9, rcvClkDrift = 4.5e-9; + const Point3 lever(0.5, -0.3, 1.0), omega(0.0, 0.0, 0.0); + const Pose3 pose(Rot3::RzRyRx(0.3, -0.2, 0.5), sample::kReceiverPos); + + const auto arm = DopplerFactorArm(0, 1, 2, measDoppler, kLambdaL1, + sample::kSatPos, satVel, + sample::kReceiverPos, lever, omega, + satClkDrift); + const auto base = DopplerFactor(1, 2, measDoppler, kLambdaL1, sample::kSatPos, + satVel, sample::kReceiverPos, satClkDrift); + EXPECT_DOUBLES_EQUAL(base.evaluateError((Vector3)rcvVel, rcvClkDrift)[0], + arm.evaluateError(pose, (Vector3)rcvVel, rcvClkDrift)[0], + 1e-9); +} + +// ************************************************************************* +TEST(TestDopplerFactorArm, Model) { + const Point3 satVel(-1200.0, 2400.0, 800.0); + const Vector3 rcvVel(0.3, -0.1, 0.05); + const double measDoppler = -1500.0, satClkDrift = 1.2e-9, rcvClkDrift = 4.5e-9; + const Point3 lever(0.5, -0.3, 1.0), omega(0.02, -0.05, 0.1); + const Pose3 pose(Rot3::RzRyRx(0.3, -0.2, 0.5), sample::kReceiverPos); + + const auto factor = DopplerFactorArm(0, 1, 2, measDoppler, kLambdaL1, + sample::kSatPos, satVel, + sample::kReceiverPos, lever, omega, + satClkDrift); + + // Independent reference: antenna velocity v_ant = v + R*(omega x lever), + // then the same range-rate model (incl. Sagnac) as DopplerFactor at v_ant. + Point3 e; + gnss::geodist(sample::kSatPos, sample::kReceiverPos, e); + const Vector3 vAnt = (Vector3)rcvVel + pose.rotation().rotate(omega.cross(lever)); + const double kSag = gnss::OMGE / kCLight; + const double sagnac = + kSag * (satVel.y() * sample::kReceiverPos.x() + + sample::kSatPos.y() * vAnt.x() - + satVel.x() * sample::kReceiverPos.y() - + sample::kSatPos.x() * vAnt.y()); + const double rangeRate = e.dot(satVel - Point3(vAnt)) + + kCLight * (rcvClkDrift - satClkDrift) + sagnac; + const double expected = rangeRate - (-kLambdaL1 * measDoppler); + EXPECT_DOUBLES_EQUAL( + expected, factor.evaluateError(pose, (Vector3)rcvVel, rcvClkDrift)[0], + 1e-6); + + Values values; + values.insert(0, pose); + values.insert(1, (Vector3)rcvVel); + values.insert(2, rcvClkDrift); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-6); +} + +// ************************************************************************* +// Local nav-frame pose overload (ecef_T_nav): checks the composed-rotation +// Jacobian path. +TEST(TestDopplerFactorArm, NavFrameJacobians) { + const Point3 satVel(-1200.0, 2400.0, 800.0); + const Vector3 rcvVel(0.3, -0.1, 0.05); + const double measDoppler = -1500.0, rcvClkDrift = 4.5e-9; + const Point3 lever(0.5, -0.3, 1.0), omega(0.02, -0.05, 0.1); + const Pose3 ecef_T_nav(Rot3::RzRyRx(0.1, 0.4, -0.7), sample::kReceiverPos); + const Pose3 navPose(Rot3::RzRyRx(0.3, -0.2, 0.5), Point3(0.0, 0.0, 0.0)); + + const auto factor = DopplerFactorArm(0, 1, 2, measDoppler, kLambdaL1, + sample::kSatPos, satVel, + sample::kReceiverPos, lever, ecef_T_nav, + omega, 0.0); + Values values; + values.insert(0, navPose); + values.insert(1, (Vector3)rcvVel); + values.insert(2, rcvClkDrift); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-6); +} + +// ************************************************************************* +TEST(TestDopplerFactorArm, equals) { + const Point3 satVel(100, 200, 300), lever(0.5, -0.3, 1.0), omega(0.02, 0, 0.1); + const auto f1 = DopplerFactorArm(0, 1, 2, 10.0, kLambdaL1, sample::kSatPos, + satVel, sample::kReceiverPos, lever, omega, 0.0); + const auto f2 = DopplerFactorArm(0, 1, 2, 10.0, kLambdaL1, sample::kSatPos, + satVel, sample::kReceiverPos, lever, omega, 0.0); + const auto f3 = DopplerFactorArm(0, 1, 2, 10.0, kLambdaL1, sample::kSatPos, + satVel, sample::kReceiverPos, + Point3(1.0, 0.0, 0.0), omega, 0.0); + CHECK(f1.equals(f2)); + CHECK(!f1.equals(f3)); // differs only in the lever arm + f1.print("dopplerArm "); +} + // ************************************************************************* int main() { TestResult tr; diff --git a/gtsam/navigation/tests/testSerializationNavigation.cpp b/gtsam/navigation/tests/testSerializationNavigation.cpp index e810b72485..60376b48c3 100644 --- a/gtsam/navigation/tests/testSerializationNavigation.cpp +++ b/gtsam/navigation/tests/testSerializationNavigation.cpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -324,6 +325,42 @@ TEST(DoubleDifferenceCarrierPhaseFactorArm, Serialization) { EXPECT(equalsBinary(f)); } +/* ************************************************************************* */ +TEST(DopplerFactor, Serialization) { + DopplerFactor f(0, 1, -1500.0, kLam, kSat1, Point3(-1200, 2400, 800), kSat1, + 1.2e-9, kGnss); + EXPECT(equalsObj(f)); + EXPECT(equalsXML(f)); + EXPECT(equalsBinary(f)); +} + +/* ************************************************************************* */ +TEST(DopplerFactorArm, Serialization) { + DopplerFactorArm f(0, 1, 2, -1500.0, kLam, kSat1, Point3(-1200, 2400, 800), + kSat1, kLever, Point3(0.02, -0.05, 0.1), 1.2e-9, kGnss); + EXPECT(equalsObj(f)); + EXPECT(equalsXML(f)); + EXPECT(equalsBinary(f)); +} + +/* ************************************************************************* */ +TEST(DopplerFactorArm, SerializationNavFrame) { + DopplerFactorArm f(0, 1, 2, -1500.0, kLam, kSat1, Point3(-1200, 2400, 800), + kSat1, kLever, Pose3(Rot3::RzRyRx(0.1, 0.4, -0.7), kSat1), + Point3(0.02, -0.05, 0.1), 1.2e-9, kGnss); + EXPECT(equalsObj(f)); + EXPECT(equalsXML(f)); + EXPECT(equalsBinary(f)); +} + +/* ************************************************************************* */ +TEST(ClockDriftFactor, Serialization) { + ClockDriftFactor f(0, 1, 2, 0.2, kGnss); + EXPECT(equalsObj(f)); + EXPECT(equalsXML(f)); + EXPECT(equalsBinary(f)); +} + /* ************************************************************************* */ int main() { TestResult tr; From 2e9d31943d1aaabfb564559900c7c9d5b10352f5 Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 3 Jul 2026 01:03:05 +0000 Subject: [PATCH 4/8] navigation: key DopplerFactor on time-differenced clock biases, drop 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 Claude-Session: https://claude.ai/code/session_01RUiPXeNrWr3cmpb2Lsa3HK --- gtsam/navigation/DopplerFactor.cpp | 133 ++++++------- gtsam/navigation/DopplerFactor.h | 183 +++++++----------- gtsam/navigation/navigation.i | 45 ++--- gtsam/navigation/tests/testDopplerFactor.cpp | 137 +++++++------ .../tests/testSerializationNavigation.cpp | 24 +-- 5 files changed, 245 insertions(+), 277 deletions(-) diff --git a/gtsam/navigation/DopplerFactor.cpp b/gtsam/navigation/DopplerFactor.cpp index 451b017ef3..fd859119d0 100644 --- a/gtsam/navigation/DopplerFactor.cpp +++ b/gtsam/navigation/DopplerFactor.cpp @@ -1,29 +1,36 @@ /** * @file DopplerFactor.cpp - * @brief Implementation of the GNSS Doppler factor and constant-drift clock + * @brief Implementation of the GNSS Doppler (range-rate) factor * @date June 17, 2026 **/ #include "DopplerFactor.h" +#include + namespace gtsam { using gnss::C_LIGHT; using gnss::OMGE; //*************************************************************************** -DopplerFactor::DopplerFactor(const Key velocityKey, const Key clockDriftKey, +DopplerFactor::DopplerFactor(const Key velocityKey, const Key clockBiasPrevKey, + const Key clockBiasCurrKey, const double measuredDoppler, const double wavelength, const Point3& satellitePosition, const Point3& satelliteVelocity, - const Point3& receiverPosition, + const Point3& receiverPosition, const double dt, const double satelliteClockDrift, const SharedNoiseModel& model) - : Base(model, velocityKey, clockDriftKey), + : Base(model, velocityKey, clockBiasPrevKey, clockBiasCurrKey), measRangeRate_(-wavelength * measuredDoppler), satVel_(satelliteVelocity), - satClkDrift_(satelliteClockDrift) { + satClkDrift_(satelliteClockDrift), + dt_(dt) { + if (!(dt > 0.0)) + throw std::invalid_argument("DopplerFactor: dt must be positive"); + // Line-of-sight unit vector (receiver -> satellite), Sagnac-aware geodist. Point3 e; gnss::geodist(satellitePosition, receiverPosition, e); @@ -46,6 +53,7 @@ void DopplerFactor::print(const std::string& s, gtsam::print(Vector(satVel_), "sat velocity (ECEF m/s): "); gtsam::print(Vector(los_), "line-of-sight (rcv->sat): "); gtsam::print(satClkDrift_, "sat clock drift (s/s): "); + gtsam::print(dt_, "epoch interval dt (s): "); gtsam::print(Vector(velSagnac_), "Sagnac rate coeff (1/s): "); gtsam::print(sagnacOffset_, "Sagnac rate offset (m/s): "); } @@ -58,18 +66,23 @@ bool DopplerFactor::equals(const NonlinearFactor& expected, double tol) const { traits::Equals(satVel_, e->satVel_, tol) && traits::Equals(los_, e->los_, tol) && traits::Equals(satClkDrift_, e->satClkDrift_, tol) && + traits::Equals(dt_, e->dt_, tol) && traits::Equals(velSagnac_, e->velSagnac_, tol) && traits::Equals(sagnacOffset_, e->sagnacOffset_, tol); } //*************************************************************************** Vector DopplerFactor::evaluateError(const Vector3& velocity, - const double& clockDrift, + const double& clockBiasPrev, + const double& clockBiasCurr, OptionalMatrixType Hvelocity, - OptionalMatrixType HclockDrift) const { - // range rate = e . (v_s - v_r) + c * (ddt_r - ddt_s) + sagnac_rate + OptionalMatrixType HclockBiasPrev, + OptionalMatrixType HclockBiasCurr) const { + // range rate = e . (v_s - v_r) + // + c * ((bias_k - bias_{k-1})/dt - ddt_s) + sagnac_rate + const double drift = (clockBiasCurr - clockBiasPrev) / dt_; const double rangeRate = los_.dot(satVel_ - velocity) + - C_LIGHT * (clockDrift - satClkDrift_) + + C_LIGHT * (drift - satClkDrift_) + sagnacOffset_ + velSagnac_.dot(velocity); const double error = rangeRate - measRangeRate_; @@ -77,8 +90,11 @@ Vector DopplerFactor::evaluateError(const Vector3& velocity, // d/d v_r [ e . (v_s - v_r) + velSagnac . v_r ] = (velSagnac - e)^T *Hvelocity = (velSagnac_ - los_).transpose(); } - if (HclockDrift) { - *HclockDrift = I_1x1 * C_LIGHT; + if (HclockBiasPrev) { + *HclockBiasPrev = -I_1x1 * (C_LIGHT / dt_); + } + if (HclockBiasCurr) { + *HclockBiasCurr = I_1x1 * (C_LIGHT / dt_); } return Vector1(error); @@ -107,34 +123,41 @@ void initDopplerArmGeometry(const Point3& satellitePosition, } // namespace DopplerFactorArm::DopplerFactorArm( - const Key poseKey, const Key velocityKey, const Key clockDriftKey, - const double measuredDoppler, const double wavelength, - const Point3& satellitePosition, const Point3& satelliteVelocity, - const Point3& receiverPosition, const Point3& leverArm, - const Point3& angularVelocity, const double satelliteClockDrift, - const SharedNoiseModel& model) - : Base(model, poseKey, velocityKey, clockDriftKey), + const Key poseKey, const Key velocityKey, const Key clockBiasPrevKey, + const Key clockBiasCurrKey, const double measuredDoppler, + const double wavelength, const Point3& satellitePosition, + const Point3& satelliteVelocity, const Point3& receiverPosition, + const Point3& leverArm, const Point3& angularVelocity, const double dt, + const double satelliteClockDrift, const SharedNoiseModel& model) + : Base(model, poseKey, velocityKey, clockBiasPrevKey, clockBiasCurrKey), measRangeRate_(-wavelength * measuredDoppler), satVel_(satelliteVelocity), satClkDrift_(satelliteClockDrift), + dt_(dt), arm_(leverArm) { + if (!(dt > 0.0)) + throw std::invalid_argument("DopplerFactorArm: dt must be positive"); initDopplerArmGeometry(satellitePosition, receiverPosition, satelliteVelocity, angularVelocity, leverArm, los_, velSagnac_, sagnacOffset_, leverVel_); } DopplerFactorArm::DopplerFactorArm( - const Key poseKey, const Key velocityKey, const Key clockDriftKey, - const double measuredDoppler, const double wavelength, - const Point3& satellitePosition, const Point3& satelliteVelocity, - const Point3& receiverPosition, const Point3& leverArm, - const Pose3& ecef_T_nav, const Point3& angularVelocity, + const Key poseKey, const Key velocityKey, const Key clockBiasPrevKey, + const Key clockBiasCurrKey, const double measuredDoppler, + const double wavelength, const Point3& satellitePosition, + const Point3& satelliteVelocity, const Point3& receiverPosition, + const Point3& leverArm, const Pose3& ecef_T_nav, + const Point3& angularVelocity, const double dt, const double satelliteClockDrift, const SharedNoiseModel& model) - : Base(model, poseKey, velocityKey, clockDriftKey), + : Base(model, poseKey, velocityKey, clockBiasPrevKey, clockBiasCurrKey), measRangeRate_(-wavelength * measuredDoppler), satVel_(satelliteVelocity), satClkDrift_(satelliteClockDrift), + dt_(dt), arm_(leverArm, ecef_T_nav) { + if (!(dt > 0.0)) + throw std::invalid_argument("DopplerFactorArm: dt must be positive"); initDopplerArmGeometry(satellitePosition, receiverPosition, satelliteVelocity, angularVelocity, leverArm, los_, velSagnac_, sagnacOffset_, leverVel_); @@ -148,6 +171,7 @@ void DopplerFactorArm::print(const std::string& s, gtsam::print(Vector(satVel_), "sat velocity (ECEF m/s): "); gtsam::print(Vector(los_), "line-of-sight (rcv->sat): "); gtsam::print(satClkDrift_, "sat clock drift (s/s): "); + gtsam::print(dt_, "epoch interval dt (s): "); gtsam::print(sagnacOffset_, "Sagnac rate offset (m/s): "); gtsam::print(Vector(arm_.b), "lever arm (body m): "); gtsam::print(Vector(leverVel_), "lever velocity omega x b (m/s): "); @@ -162,6 +186,7 @@ bool DopplerFactorArm::equals(const NonlinearFactor& expected, traits::Equals(satVel_, e->satVel_, tol) && traits::Equals(los_, e->los_, tol) && traits::Equals(satClkDrift_, e->satClkDrift_, tol) && + traits::Equals(dt_, e->dt_, tol) && traits::Equals(velSagnac_, e->velSagnac_, tol) && traits::Equals(sagnacOffset_, e->sagnacOffset_, tol) && arm_.equals(e->arm_, tol) && @@ -169,12 +194,11 @@ bool DopplerFactorArm::equals(const NonlinearFactor& expected, } //*************************************************************************** -Vector DopplerFactorArm::evaluateError(const Pose3& pose, - const Vector3& velocity, - const double& clockDrift, - OptionalMatrixType Hpose, - OptionalMatrixType Hvelocity, - OptionalMatrixType HclockDrift) const { +Vector DopplerFactorArm::evaluateError( + const Pose3& pose, const Vector3& velocity, const double& clockBiasPrev, + const double& clockBiasCurr, OptionalMatrixType Hpose, + OptionalMatrixType Hvelocity, OptionalMatrixType HclockBiasPrev, + OptionalMatrixType HclockBiasCurr) const { // Lever-arm velocity in ECEF: v_lever = ecef_R_body * (omega x leverArm). // Hrot is d(v_lever)/d(rotation tangent) [3x3]. Matrix3 Hrot; @@ -192,8 +216,9 @@ Vector DopplerFactorArm::evaluateError(const Pose3& pose, // Effective range-rate coefficient on the antenna velocity: (velSagnac - e). const Vector3 g = Vector3(velSagnac_) - Vector3(los_); + const double drift = (clockBiasCurr - clockBiasPrev) / dt_; const double rangeRate = los_.dot(satVel_) + - C_LIGHT * (clockDrift - satClkDrift_) + + C_LIGHT * (drift - satClkDrift_) + sagnacOffset_ + g.dot(vAnt); const double error = rangeRate - measRangeRate_; @@ -208,48 +233,12 @@ Vector DopplerFactorArm::evaluateError(const Pose3& pose, if (Hvelocity) { *Hvelocity = g.transpose(); } - if (HclockDrift) { - *HclockDrift = I_1x1 * C_LIGHT; + if (HclockBiasPrev) { + *HclockBiasPrev = -I_1x1 * (C_LIGHT / dt_); + } + if (HclockBiasCurr) { + *HclockBiasCurr = I_1x1 * (C_LIGHT / dt_); } - - return Vector1(error); -} - -//*************************************************************************** -ClockDriftFactor::ClockDriftFactor(const Key clockBiasPrevKey, - const Key clockBiasCurrKey, - const Key clockDriftKey, const double dt, - const SharedNoiseModel& model) - : Base(model, clockBiasPrevKey, clockBiasCurrKey, clockDriftKey), dt_(dt) {} - -//*************************************************************************** -void ClockDriftFactor::print(const std::string& s, - const KeyFormatter& keyFormatter) const { - Base::print(s, keyFormatter); - gtsam::print(dt_, "dt (s): "); -} - -//*************************************************************************** -bool ClockDriftFactor::equals(const NonlinearFactor& expected, - double tol) const { - const This* e = dynamic_cast(&expected); - return e != nullptr && Base::equals(*e, tol) && - traits::Equals(dt_, e->dt_, tol); -} - -//*************************************************************************** -Vector ClockDriftFactor::evaluateError(const double& clockBiasPrev, - const double& clockBiasCurr, - const double& clockDrift, - OptionalMatrixType HbiasPrev, - OptionalMatrixType HbiasCurr, - OptionalMatrixType Hdrift) const { - // Constant-velocity clock: bias(k) - bias(k-1) - drift*dt = 0 - const double error = clockBiasCurr - clockBiasPrev - clockDrift * dt_; - - if (HbiasPrev) *HbiasPrev = -I_1x1; - if (HbiasCurr) *HbiasCurr = I_1x1; - if (Hdrift) *Hdrift = -I_1x1 * dt_; return Vector1(error); } diff --git a/gtsam/navigation/DopplerFactor.h b/gtsam/navigation/DopplerFactor.h index 70d2be4af7..1159969181 100644 --- a/gtsam/navigation/DopplerFactor.h +++ b/gtsam/navigation/DopplerFactor.h @@ -18,16 +18,27 @@ namespace gtsam { /** * GNSS Doppler (range-rate) factor. * - * Relates a measured Doppler observation to the receiver velocity and clock - * drift, reusing the line-of-sight unit vector from gnss::geodist: + * Relates a measured Doppler observation to the receiver velocity and the + * receiver clock, reusing the line-of-sight unit vector from gnss::geodist. + * The receiver clock drift is not a separate state: it is expressed as the + * time difference of the two adjacent receiver clock-bias states already + * estimated by the pseudorange / carrier-phase factors, * - * error = e . (v_s - v_r) + c * (ddt_r - ddt_s) + sagnac_rate + * drift ~= (dt_r(k) - dt_r(k-1)) / dt + * + * so the factor reads + * + * error = e . (v_s - v_r) + * + c * ((dt_r(k) - dt_r(k-1)) / dt - ddt_s) + * + sagnac_rate * - (-lambda * Doppler) * * where * - e is the unit line-of-sight vector from receiver to satellite, * - v_s, v_r are the satellite and receiver ECEF velocities (m/s), - * - ddt_r is the receiver clock drift (s/s) -- the estimated state, + * - dt_r(k-1), dt_r(k) are the receiver clock biases (s) at the previous + * and current epochs -- the estimated states, + * - dt is the epoch interval t_k - t_{k-1} (s), * - ddt_s is the satellite clock drift (s/s), * - sagnac_rate = (OMGE/c) * (v_s.y*r_r.x + r_s.y*v_r.x * - v_s.x*r_r.y - r_s.x*v_r.y) @@ -37,25 +48,36 @@ namespace gtsam { * (approaching satellite) corresponds to a decreasing range, hence the * leading minus sign. * + * Keying on the two clock biases (instead of a dedicated clock-drift state) + * keeps the state vector identical to the pseudorange-only problem and needs + * no extra between-epoch clock factor: the Doppler measurement itself + * constrains the clock-bias evolution. The instantaneous drift is + * approximated by its average over [t_{k-1}, t_k], which is the standard + * time-differenced formulation (same structure as TDCP) and is accurate for + * the ~1 s epoch intervals typical of GNSS receivers. + * * The satellite position/velocity and the line-of-sight are held constant per * factor; only the (second-order) dependence of the line-of-sight on the * receiver position is neglected, which is standard for Doppler velocity - * estimation. Doppler provides receiver - * velocity and clock-drift observability that is robust through carrier-phase - * cycle slips and re-convergence. + * estimation. Doppler provides receiver velocity and clock observability + * that is robust through carrier-phase cycle slips and re-convergence. * - * Keys: [velocity (Vector3, m/s), receiver clock drift (double, s/s)]. + * Keys: [velocity (Vector3, m/s), + * receiver clock bias at epoch k-1 (double, s), + * receiver clock bias at epoch k (double, s)]. * * @ingroup navigation */ -class GTSAM_EXPORT DopplerFactor : public NoiseModelFactorN { +class GTSAM_EXPORT DopplerFactor + : public NoiseModelFactorN { private: - typedef NoiseModelFactorN Base; + typedef NoiseModelFactorN Base; double measRangeRate_ = 0.0; ///< Measured range rate = -lambda*D [m/s]. Point3 satVel_{0, 0, 0}; ///< Satellite ECEF velocity [m/s]. Point3 los_{0, 0, 0}; ///< Unit LOS, receiver -> satellite. double satClkDrift_ = 0.0; ///< Satellite clock drift [s/s]. + double dt_ = 1.0; ///< Epoch interval t_k - t_{k-1} [s]. Point3 velSagnac_{0, 0, 0}; ///< Sagnac rate coeff, d(rate)/d(v_r). double sagnacOffset_ = 0.0; ///< v_r-independent Sagnac rate term [m/s]. @@ -70,19 +92,22 @@ class GTSAM_EXPORT DopplerFactor : public NoiseModelFactorN { /** * @param velocityKey Receiver ECEF velocity node (Vector3, m/s). - * @param clockDriftKey Receiver clock drift node (double, s/s). + * @param clockBiasPrevKey Receiver clock bias node at epoch k-1 (s). + * @param clockBiasCurrKey Receiver clock bias node at epoch k (s). * @param measuredDoppler Measured Doppler [Hz]. * @param wavelength Carrier wavelength [m/cycle]. * @param satellitePosition Satellite ECEF position [m] (for the LOS). * @param satelliteVelocity Satellite ECEF velocity [m/s]. * @param receiverPosition Receiver ECEF position [m] (for the LOS). + * @param dt Epoch interval t_k - t_{k-1} [s], must be > 0. * @param satelliteClockDrift Satellite clock drift [s/s]. * @param model 1-D range-rate noise model. */ - DopplerFactor(Key velocityKey, Key clockDriftKey, double measuredDoppler, - double wavelength, const Point3& satellitePosition, + DopplerFactor(Key velocityKey, Key clockBiasPrevKey, Key clockBiasCurrKey, + double measuredDoppler, double wavelength, + const Point3& satellitePosition, const Point3& satelliteVelocity, const Point3& receiverPosition, - double satelliteClockDrift = 0.0, + double dt, double satelliteClockDrift = 0.0, const SharedNoiseModel& model = noiseModel::Unit::Create(1)); gtsam::NonlinearFactor::shared_ptr clone() const override { @@ -95,14 +120,18 @@ class GTSAM_EXPORT DopplerFactor : public NoiseModelFactorN { bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; - Vector evaluateError(const Vector3& velocity, const double& clockDrift, + Vector evaluateError(const Vector3& velocity, const double& clockBiasPrev, + const double& clockBiasCurr, OptionalMatrixType Hvelocity, - OptionalMatrixType HclockDrift) const override; + OptionalMatrixType HclockBiasPrev, + OptionalMatrixType HclockBiasCurr) const override; /// Measured range rate (= -lambda * Doppler) [m/s]. inline double measuredRangeRate() const { return measRangeRate_; } /// Unit line-of-sight vector (receiver -> satellite). inline const Point3& lineOfSight() const { return los_; } + /// Epoch interval t_k - t_{k-1} [s]. + inline double dt() const { return dt_; } private: #if GTSAM_ENABLE_BOOST_SERIALIZATION @@ -114,6 +143,7 @@ class GTSAM_EXPORT DopplerFactor : public NoiseModelFactorN { ar& BOOST_SERIALIZATION_NVP(satVel_); ar& BOOST_SERIALIZATION_NVP(los_); ar& BOOST_SERIALIZATION_NVP(satClkDrift_); + ar& BOOST_SERIALIZATION_NVP(dt_); ar& BOOST_SERIALIZATION_NVP(velSagnac_); ar& BOOST_SERIALIZATION_NVP(sagnacOffset_); } @@ -138,8 +168,12 @@ struct traits : public Testable {}; * where omega is the (measured) body-frame angular velocity and leverArm is the * body-frame antenna offset. The range-rate error then uses v_antenna: * - * error = e . (v_s - v_antenna) + c*(ddt_r - ddt_s) + sagnac_rate - * - (-lambda*Doppler) + * error = e . (v_s - v_antenna) + * + c * ((dt_r(k) - dt_r(k-1)) / dt - ddt_s) + * + sagnac_rate - (-lambda*Doppler) + * + * with the receiver clock drift expressed through the two adjacent clock-bias + * states, exactly as in DopplerFactor. * * The line-of-sight e and the Sagnac terms are evaluated at the provided * (nominal) receiver position, exactly as in DopplerFactor -- the second-order @@ -151,19 +185,21 @@ struct traits : public Testable {}; * navigation-frame pose (e.g. ENU) and ecef_R_body = ecef_R_nav * nav_R_body; * `velocity` is still the receiver ECEF velocity. * - * Keys: [pose (Pose3), velocity (Vector3, ECEF m/s), clock drift (double, s/s)]. + * Keys: [pose (Pose3), velocity (Vector3, ECEF m/s), + * clock bias k-1 (double, s), clock bias k (double, s)]. * * @ingroup navigation */ class GTSAM_EXPORT DopplerFactorArm - : public NoiseModelFactorN { + : public NoiseModelFactorN { private: - typedef NoiseModelFactorN Base; + typedef NoiseModelFactorN Base; double measRangeRate_ = 0.0; ///< Measured range rate = -lambda*D [m/s]. Point3 satVel_{0, 0, 0}; ///< Satellite ECEF velocity [m/s]. Point3 los_{0, 0, 0}; ///< Unit LOS, receiver -> satellite. double satClkDrift_ = 0.0; ///< Satellite clock drift [s/s]. + double dt_ = 1.0; ///< Epoch interval t_k - t_{k-1} [s]. Point3 velSagnac_{0, 0, 0}; ///< Sagnac rate coeff, d(rate)/d(v_ant). double sagnacOffset_ = 0.0; ///< v_ant-independent Sagnac rate term [m/s]. gnss::LeverArm arm_; ///< Lever arm (body frame) + optional ecef_T_nav. @@ -183,7 +219,8 @@ class GTSAM_EXPORT DopplerFactorArm * * @param poseKey Receiver body Pose3 (ECEF) node. * @param velocityKey Receiver ECEF velocity node (Vector3, m/s). - * @param clockDriftKey Receiver clock drift node (double, s/s). + * @param clockBiasPrevKey Receiver clock bias node at epoch k-1 (s). + * @param clockBiasCurrKey Receiver clock bias node at epoch k (s). * @param measuredDoppler Measured Doppler [Hz]. * @param wavelength Carrier wavelength [m/cycle]. * @param satellitePosition Satellite ECEF position [m] (for the LOS). @@ -191,26 +228,27 @@ class GTSAM_EXPORT DopplerFactorArm * @param receiverPosition Nominal receiver ECEF position [m] (for the LOS). * @param leverArm Antenna lever arm in the body frame [m]. * @param angularVelocity Body-frame angular velocity omega [rad/s]. + * @param dt Epoch interval t_k - t_{k-1} [s], must be > 0. * @param satelliteClockDrift Satellite clock drift [s/s]. * @param model 1-D range-rate noise model. */ - DopplerFactorArm(Key poseKey, Key velocityKey, Key clockDriftKey, - double measuredDoppler, double wavelength, - const Point3& satellitePosition, + DopplerFactorArm(Key poseKey, Key velocityKey, Key clockBiasPrevKey, + Key clockBiasCurrKey, double measuredDoppler, + double wavelength, const Point3& satellitePosition, const Point3& satelliteVelocity, const Point3& receiverPosition, const Point3& leverArm, - const Point3& angularVelocity, + const Point3& angularVelocity, double dt, double satelliteClockDrift = 0.0, const SharedNoiseModel& model = noiseModel::Unit::Create(1)); /// Construct with a local nav-frame pose key + ecef_T_nav. - DopplerFactorArm(Key poseKey, Key velocityKey, Key clockDriftKey, - double measuredDoppler, double wavelength, - const Point3& satellitePosition, + DopplerFactorArm(Key poseKey, Key velocityKey, Key clockBiasPrevKey, + Key clockBiasCurrKey, double measuredDoppler, + double wavelength, const Point3& satellitePosition, const Point3& satelliteVelocity, const Point3& receiverPosition, const Point3& leverArm, const Pose3& ecef_T_nav, const Point3& angularVelocity, - double satelliteClockDrift = 0.0, + double dt, double satelliteClockDrift = 0.0, const SharedNoiseModel& model = noiseModel::Unit::Create(1)); gtsam::NonlinearFactor::shared_ptr clone() const override { @@ -224,14 +262,17 @@ class GTSAM_EXPORT DopplerFactorArm double tol = 1e-9) const override; Vector evaluateError(const Pose3& pose, const Vector3& velocity, - const double& clockDrift, OptionalMatrixType Hpose, - OptionalMatrixType Hvelocity, - OptionalMatrixType HclockDrift) const override; + const double& clockBiasPrev, const double& clockBiasCurr, + OptionalMatrixType Hpose, OptionalMatrixType Hvelocity, + OptionalMatrixType HclockBiasPrev, + OptionalMatrixType HclockBiasCurr) const override; /// Measured range rate (= -lambda * Doppler) [m/s]. inline double measuredRangeRate() const { return measRangeRate_; } /// Unit line-of-sight vector (receiver -> satellite). inline const Point3& lineOfSight() const { return los_; } + /// Epoch interval t_k - t_{k-1} [s]. + inline double dt() const { return dt_; } /// Lever arm in the body frame [m]. inline const Point3& leverArm() const { return arm_.b; } /// Optional ECEF-from-nav transform. @@ -249,6 +290,7 @@ class GTSAM_EXPORT DopplerFactorArm ar& BOOST_SERIALIZATION_NVP(satVel_); ar& BOOST_SERIALIZATION_NVP(los_); ar& BOOST_SERIALIZATION_NVP(satClkDrift_); + ar& BOOST_SERIALIZATION_NVP(dt_); ar& BOOST_SERIALIZATION_NVP(velSagnac_); ar& BOOST_SERIALIZATION_NVP(sagnacOffset_); ar& boost::serialization::make_nvp("bL_", arm_.b); @@ -262,79 +304,4 @@ class GTSAM_EXPORT DopplerFactorArm template <> struct traits : public Testable {}; -/** - * Constant-drift receiver clock factor. - * - * Links two consecutive receiver clock biases through a (constant) clock drift, - * i.e. the constant-velocity clock model -- the scalar analog of how the IMU - * factor links position and velocity: - * - * error = bias(k) - bias(k-1) - drift * dt - * - * The drift state is shared by the DopplerFactor (which observes it) and by this - * factor (which integrates it into the clock bias used by the pseudorange / - * carrier-phase factors), so Doppler velocity information propagates into the - * clock bias. Bias is in seconds, drift in seconds/second, dt in seconds. - * - * Keys: [clock bias prev (double), clock bias curr (double), drift (double)]. - * - * @ingroup navigation - */ -class GTSAM_EXPORT ClockDriftFactor - : public NoiseModelFactorN { - private: - typedef NoiseModelFactorN Base; - double dt_ = 0.0; ///< Time step between the two clock-bias epochs [s]. - - public: - using Base::evaluateError; - typedef std::shared_ptr shared_ptr; - typedef ClockDriftFactor This; - - ClockDriftFactor() = default; - virtual ~ClockDriftFactor() = default; - - /** - * @param clockBiasPrevKey Receiver clock bias at epoch k-1 [s]. - * @param clockBiasCurrKey Receiver clock bias at epoch k [s]. - * @param clockDriftKey Receiver clock drift [s/s]. - * @param dt Time step (t_k - t_{k-1}) [s]. - * @param model 1-D noise model (clock process noise). - */ - ClockDriftFactor(Key clockBiasPrevKey, Key clockBiasCurrKey, - Key clockDriftKey, double dt, - const SharedNoiseModel& model = noiseModel::Unit::Create(1)); - - gtsam::NonlinearFactor::shared_ptr clone() const override { - return std::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); - } - - void print(const std::string& s = "", const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const override; - bool equals(const NonlinearFactor& expected, - double tol = 1e-9) const override; - - Vector evaluateError(const double& clockBiasPrev, const double& clockBiasCurr, - const double& clockDrift, OptionalMatrixType HbiasPrev, - OptionalMatrixType HbiasCurr, - OptionalMatrixType Hdrift) const override; - - inline double dt() const { return dt_; } - - private: -#if GTSAM_ENABLE_BOOST_SERIALIZATION - friend class boost::serialization::access; - template - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar& BOOST_SERIALIZATION_NVP(dt_); - } -#endif -}; - -/// traits -template <> -struct traits : public Testable {}; - } // namespace gtsam diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 715b01f0f6..39a2e91aec 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -730,11 +730,11 @@ virtual class DoubleDifferenceCarrierPhaseFactorArm : gtsam::NonlinearFactor { #include virtual class DopplerFactor : gtsam::NonlinearFactor { - DopplerFactor(gtsam::Key velocityKey, gtsam::Key clockDriftKey, - double measuredDoppler, double wavelength, - const gtsam::Point3& satellitePosition, + DopplerFactor(gtsam::Key velocityKey, gtsam::Key clockBiasPrevKey, + gtsam::Key clockBiasCurrKey, double measuredDoppler, + double wavelength, const gtsam::Point3& satellitePosition, const gtsam::Point3& satelliteVelocity, - const gtsam::Point3& receiverPosition, + const gtsam::Point3& receiverPosition, double dt, double satelliteClockDrift, const gtsam::noiseModel::Base* model); @@ -742,29 +742,33 @@ virtual class DopplerFactor : gtsam::NonlinearFactor { gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::NonlinearFactor& expected, double tol) const; gtsam::Vector evaluateError(const gtsam::Vector3& velocity, - const double& clockDrift) const; + const double& clockBiasPrev, + const double& clockBiasCurr) const; double measuredRangeRate() const; const gtsam::Point3& lineOfSight() const; + double dt() const; void serialize() const; }; virtual class DopplerFactorArm : gtsam::NonlinearFactor { DopplerFactorArm(gtsam::Key poseKey, gtsam::Key velocityKey, - gtsam::Key clockDriftKey, double measuredDoppler, - double wavelength, const gtsam::Point3& satellitePosition, + gtsam::Key clockBiasPrevKey, gtsam::Key clockBiasCurrKey, + double measuredDoppler, double wavelength, + const gtsam::Point3& satellitePosition, const gtsam::Point3& satelliteVelocity, const gtsam::Point3& receiverPosition, const gtsam::Point3& leverArm, - const gtsam::Point3& angularVelocity, + const gtsam::Point3& angularVelocity, double dt, double satelliteClockDrift, const gtsam::noiseModel::Base* model); DopplerFactorArm(gtsam::Key poseKey, gtsam::Key velocityKey, - gtsam::Key clockDriftKey, double measuredDoppler, - double wavelength, const gtsam::Point3& satellitePosition, + gtsam::Key clockBiasPrevKey, gtsam::Key clockBiasCurrKey, + double measuredDoppler, double wavelength, + const gtsam::Point3& satellitePosition, const gtsam::Point3& satelliteVelocity, const gtsam::Point3& receiverPosition, const gtsam::Point3& leverArm, const gtsam::Pose3& ecef_T_nav, - const gtsam::Point3& angularVelocity, + const gtsam::Point3& angularVelocity, double dt, double satelliteClockDrift, const gtsam::noiseModel::Base* model); @@ -773,25 +777,12 @@ virtual class DopplerFactorArm : gtsam::NonlinearFactor { bool equals(const gtsam::NonlinearFactor& expected, double tol) const; gtsam::Vector evaluateError(const gtsam::Pose3& pose, const gtsam::Vector3& velocity, - const double& clockDrift) const; + const double& clockBiasPrev, + const double& clockBiasCurr) const; double measuredRangeRate() const; const gtsam::Point3& lineOfSight() const; - const gtsam::Point3& leverArm() const; - void serialize() const; -}; - -virtual class ClockDriftFactor : gtsam::NonlinearFactor { - ClockDriftFactor(gtsam::Key clockBiasPrevKey, gtsam::Key clockBiasCurrKey, - gtsam::Key clockDriftKey, double dt, - const gtsam::noiseModel::Base* model); - - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::NonlinearFactor& expected, double tol) const; - gtsam::Vector evaluateError(const double& clockBiasPrev, - const double& clockBiasCurr, - const double& clockDrift) const; double dt() const; + const gtsam::Point3& leverArm() const; void serialize() const; }; diff --git a/gtsam/navigation/tests/testDopplerFactor.cpp b/gtsam/navigation/tests/testDopplerFactor.cpp index c2146b965f..95a6b5769d 100644 --- a/gtsam/navigation/tests/testDopplerFactor.cpp +++ b/gtsam/navigation/tests/testDopplerFactor.cpp @@ -1,6 +1,6 @@ /** * @file testDopplerFactor.cpp - * @brief Unit tests for DopplerFactor and ClockDriftFactor + * @brief Unit tests for DopplerFactor and DopplerFactorArm * @date June 17, 2026 */ @@ -22,16 +22,20 @@ TEST(TestDopplerFactor, Model) { const double measDoppler = -1500.0; // [Hz] const double satClkDrift = 1.2e-9; // [s/s] const double rcvClkDrift = 4.5e-9; // [s/s] + const double dt = 0.2; // epoch interval [s] + const double biasPrev = 1.0e-6; // [s] + const double biasCurr = biasPrev + rcvClkDrift * dt; const auto factor = DopplerFactor( - Key(0), Key(1), measDoppler, kLambdaL1, sample::kSatPos, satVel, - sample::kReceiverPos, satClkDrift); + Key(0), Key(1), Key(2), measDoppler, kLambdaL1, sample::kSatPos, satVel, + sample::kReceiverPos, dt, satClkDrift); const double error = - factor.evaluateError((Vector3)rcvVel, rcvClkDrift)[0]; + factor.evaluateError((Vector3)rcvVel, biasPrev, biasCurr)[0]; // Reference: range rate via Sagnac-aware LOS, plus the earth-rotation - // (Sagnac) rate term, minus the measured range rate. + // (Sagnac) rate term, minus the measured range rate. The receiver clock + // drift is the time-differenced bias, (biasCurr - biasPrev)/dt = rcvClkDrift. Point3 e; gnss::geodist(sample::kSatPos, sample::kReceiverPos, e); const double kSag = gnss::OMGE / kCLight; @@ -47,64 +51,79 @@ TEST(TestDopplerFactor, Model) { Values values; values.insert(Key(0), (Vector3)rcvVel); - values.insert(Key(1), rcvClkDrift); + values.insert(Key(1), biasPrev); + values.insert(Key(2), biasCurr); EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-3, 1e-5); } +// ************************************************************************* +// The clock term must depend only on the bias difference: shifting both bias +// states by a common offset leaves the error unchanged (Doppler cannot +// observe the absolute bias, only its rate). +TEST(TestDopplerFactor, CommonBiasOffsetInvariance) { + const Point3 satVel(-1200.0, 2400.0, 800.0); + const Vector3 rcvVel(0.3, -0.1, 0.05); + const double dt = 1.0, biasPrev = 1.0e-6, biasCurr = 1.0045e-6; + + const auto factor = DopplerFactor(0, 1, 2, -1500.0, kLambdaL1, + sample::kSatPos, satVel, + sample::kReceiverPos, dt, 1.2e-9); + const double e1 = factor.evaluateError(rcvVel, biasPrev, biasCurr)[0]; + const double offset = 3.7e-4; // common clock offset [s] + const double e2 = + factor.evaluateError(rcvVel, biasPrev + offset, biasCurr + offset)[0]; + EXPECT_DOUBLES_EQUAL(e1, e2, 1e-6); +} + +// ************************************************************************* +TEST(TestDopplerFactor, InvalidDtThrows) { + const Point3 satVel(100, 200, 300); + CHECK_EXCEPTION(DopplerFactor(0, 1, 2, 10.0, kLambdaL1, sample::kSatPos, + satVel, sample::kReceiverPos, 0.0), + std::invalid_argument); + CHECK_EXCEPTION(DopplerFactor(0, 1, 2, 10.0, kLambdaL1, sample::kSatPos, + satVel, sample::kReceiverPos, -1.0), + std::invalid_argument); +} + // ************************************************************************* TEST(TestDopplerFactor, equals) { const Point3 satVel(100, 200, 300); - const auto f1 = DopplerFactor(0, 1, 10.0, kLambdaL1, sample::kSatPos, satVel, - sample::kReceiverPos, 0.0); - const auto f2 = DopplerFactor(0, 1, 10.0, kLambdaL1, sample::kSatPos, satVel, - sample::kReceiverPos, 0.0); - const auto f3 = DopplerFactor(0, 1, 99.0, kLambdaL1, sample::kSatPos, satVel, - sample::kReceiverPos, 0.0); + const auto f1 = DopplerFactor(0, 1, 2, 10.0, kLambdaL1, sample::kSatPos, + satVel, sample::kReceiverPos, 1.0, 0.0); + const auto f2 = DopplerFactor(0, 1, 2, 10.0, kLambdaL1, sample::kSatPos, + satVel, sample::kReceiverPos, 1.0, 0.0); + const auto f3 = DopplerFactor(0, 1, 2, 99.0, kLambdaL1, sample::kSatPos, + satVel, sample::kReceiverPos, 1.0, 0.0); + const auto f4 = DopplerFactor(0, 1, 2, 10.0, kLambdaL1, sample::kSatPos, + satVel, sample::kReceiverPos, 0.5, 0.0); CHECK(f1.equals(f2)); CHECK(!f1.equals(f3)); + CHECK(!f1.equals(f4)); // differs only in dt f1.print("doppler "); } -// ************************************************************************* -TEST(TestClockDriftFactor, Model) { - const double dt = 0.2; - const double biasPrev = 1.0e-6, drift = 4.5e-9; - const double biasCurr = biasPrev + drift * dt; // zero-error point - - const auto factor = ClockDriftFactor(Key(0), Key(1), Key(2), dt); - const double error = factor.evaluateError(biasPrev, biasCurr, drift)[0]; - EXPECT_DOUBLES_EQUAL(0.0, error, 1e-15); - - // Off the constraint: - const double error2 = factor.evaluateError(biasPrev, biasCurr + 1e-7, drift)[0]; - EXPECT_DOUBLES_EQUAL(1e-7, error2, 1e-15); - - Values values; - values.insert(Key(0), biasPrev); - values.insert(Key(1), biasCurr); - values.insert(Key(2), drift); - EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-3, 1e-6); -} - // ************************************************************************* // DopplerFactorArm: with omega = 0 it must reduce to DopplerFactor at the same // velocity, independent of the pose attitude (lever velocity omega x b = 0). TEST(TestDopplerFactorArm, ReducesToBaseWhenNoRotationRate) { const Point3 satVel(-1200.0, 2400.0, 800.0); const Vector3 rcvVel(0.3, -0.1, 0.05); - const double measDoppler = -1500.0, satClkDrift = 1.2e-9, rcvClkDrift = 4.5e-9; + const double measDoppler = -1500.0, satClkDrift = 1.2e-9; + const double dt = 0.2, biasPrev = 1.0e-6, biasCurr = 1.0e-6 + 4.5e-9 * dt; const Point3 lever(0.5, -0.3, 1.0), omega(0.0, 0.0, 0.0); const Pose3 pose(Rot3::RzRyRx(0.3, -0.2, 0.5), sample::kReceiverPos); - const auto arm = DopplerFactorArm(0, 1, 2, measDoppler, kLambdaL1, + const auto arm = DopplerFactorArm(0, 1, 2, 3, measDoppler, kLambdaL1, sample::kSatPos, satVel, - sample::kReceiverPos, lever, omega, + sample::kReceiverPos, lever, omega, dt, satClkDrift); - const auto base = DopplerFactor(1, 2, measDoppler, kLambdaL1, sample::kSatPos, - satVel, sample::kReceiverPos, satClkDrift); - EXPECT_DOUBLES_EQUAL(base.evaluateError((Vector3)rcvVel, rcvClkDrift)[0], - arm.evaluateError(pose, (Vector3)rcvVel, rcvClkDrift)[0], - 1e-9); + const auto base = DopplerFactor(1, 2, 3, measDoppler, kLambdaL1, + sample::kSatPos, satVel, + sample::kReceiverPos, dt, satClkDrift); + EXPECT_DOUBLES_EQUAL( + base.evaluateError((Vector3)rcvVel, biasPrev, biasCurr)[0], + arm.evaluateError(pose, (Vector3)rcvVel, biasPrev, biasCurr)[0], 1e-9); } // ************************************************************************* @@ -112,12 +131,14 @@ TEST(TestDopplerFactorArm, Model) { const Point3 satVel(-1200.0, 2400.0, 800.0); const Vector3 rcvVel(0.3, -0.1, 0.05); const double measDoppler = -1500.0, satClkDrift = 1.2e-9, rcvClkDrift = 4.5e-9; + const double dt = 0.2, biasPrev = 1.0e-6; + const double biasCurr = biasPrev + rcvClkDrift * dt; const Point3 lever(0.5, -0.3, 1.0), omega(0.02, -0.05, 0.1); const Pose3 pose(Rot3::RzRyRx(0.3, -0.2, 0.5), sample::kReceiverPos); - const auto factor = DopplerFactorArm(0, 1, 2, measDoppler, kLambdaL1, + const auto factor = DopplerFactorArm(0, 1, 2, 3, measDoppler, kLambdaL1, sample::kSatPos, satVel, - sample::kReceiverPos, lever, omega, + sample::kReceiverPos, lever, omega, dt, satClkDrift); // Independent reference: antenna velocity v_ant = v + R*(omega x lever), @@ -135,13 +156,15 @@ TEST(TestDopplerFactorArm, Model) { kCLight * (rcvClkDrift - satClkDrift) + sagnac; const double expected = rangeRate - (-kLambdaL1 * measDoppler); EXPECT_DOUBLES_EQUAL( - expected, factor.evaluateError(pose, (Vector3)rcvVel, rcvClkDrift)[0], + expected, + factor.evaluateError(pose, (Vector3)rcvVel, biasPrev, biasCurr)[0], 1e-6); Values values; values.insert(0, pose); values.insert(1, (Vector3)rcvVel); - values.insert(2, rcvClkDrift); + values.insert(2, biasPrev); + values.insert(3, biasCurr); EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-6); } @@ -151,32 +174,36 @@ TEST(TestDopplerFactorArm, Model) { TEST(TestDopplerFactorArm, NavFrameJacobians) { const Point3 satVel(-1200.0, 2400.0, 800.0); const Vector3 rcvVel(0.3, -0.1, 0.05); - const double measDoppler = -1500.0, rcvClkDrift = 4.5e-9; + const double measDoppler = -1500.0; + const double dt = 1.0, biasPrev = 1.0e-6, biasCurr = 1.0045e-6; const Point3 lever(0.5, -0.3, 1.0), omega(0.02, -0.05, 0.1); const Pose3 ecef_T_nav(Rot3::RzRyRx(0.1, 0.4, -0.7), sample::kReceiverPos); const Pose3 navPose(Rot3::RzRyRx(0.3, -0.2, 0.5), Point3(0.0, 0.0, 0.0)); - const auto factor = DopplerFactorArm(0, 1, 2, measDoppler, kLambdaL1, + const auto factor = DopplerFactorArm(0, 1, 2, 3, measDoppler, kLambdaL1, sample::kSatPos, satVel, sample::kReceiverPos, lever, ecef_T_nav, - omega, 0.0); + omega, dt, 0.0); Values values; values.insert(0, navPose); values.insert(1, (Vector3)rcvVel); - values.insert(2, rcvClkDrift); + values.insert(2, biasPrev); + values.insert(3, biasCurr); EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-6); } // ************************************************************************* TEST(TestDopplerFactorArm, equals) { const Point3 satVel(100, 200, 300), lever(0.5, -0.3, 1.0), omega(0.02, 0, 0.1); - const auto f1 = DopplerFactorArm(0, 1, 2, 10.0, kLambdaL1, sample::kSatPos, - satVel, sample::kReceiverPos, lever, omega, 0.0); - const auto f2 = DopplerFactorArm(0, 1, 2, 10.0, kLambdaL1, sample::kSatPos, - satVel, sample::kReceiverPos, lever, omega, 0.0); - const auto f3 = DopplerFactorArm(0, 1, 2, 10.0, kLambdaL1, sample::kSatPos, + const auto f1 = DopplerFactorArm(0, 1, 2, 3, 10.0, kLambdaL1, sample::kSatPos, + satVel, sample::kReceiverPos, lever, omega, + 1.0, 0.0); + const auto f2 = DopplerFactorArm(0, 1, 2, 3, 10.0, kLambdaL1, sample::kSatPos, + satVel, sample::kReceiverPos, lever, omega, + 1.0, 0.0); + const auto f3 = DopplerFactorArm(0, 1, 2, 3, 10.0, kLambdaL1, sample::kSatPos, satVel, sample::kReceiverPos, - Point3(1.0, 0.0, 0.0), omega, 0.0); + Point3(1.0, 0.0, 0.0), omega, 1.0, 0.0); CHECK(f1.equals(f2)); CHECK(!f1.equals(f3)); // differs only in the lever arm f1.print("dopplerArm "); diff --git a/gtsam/navigation/tests/testSerializationNavigation.cpp b/gtsam/navigation/tests/testSerializationNavigation.cpp index 60376b48c3..c081c73817 100644 --- a/gtsam/navigation/tests/testSerializationNavigation.cpp +++ b/gtsam/navigation/tests/testSerializationNavigation.cpp @@ -327,8 +327,8 @@ TEST(DoubleDifferenceCarrierPhaseFactorArm, Serialization) { /* ************************************************************************* */ TEST(DopplerFactor, Serialization) { - DopplerFactor f(0, 1, -1500.0, kLam, kSat1, Point3(-1200, 2400, 800), kSat1, - 1.2e-9, kGnss); + DopplerFactor f(0, 1, 2, -1500.0, kLam, kSat1, Point3(-1200, 2400, 800), + kSat1, 1.0, 1.2e-9, kGnss); EXPECT(equalsObj(f)); EXPECT(equalsXML(f)); EXPECT(equalsBinary(f)); @@ -336,8 +336,9 @@ TEST(DopplerFactor, Serialization) { /* ************************************************************************* */ TEST(DopplerFactorArm, Serialization) { - DopplerFactorArm f(0, 1, 2, -1500.0, kLam, kSat1, Point3(-1200, 2400, 800), - kSat1, kLever, Point3(0.02, -0.05, 0.1), 1.2e-9, kGnss); + DopplerFactorArm f(0, 1, 2, 3, -1500.0, kLam, kSat1, + Point3(-1200, 2400, 800), kSat1, kLever, + Point3(0.02, -0.05, 0.1), 1.0, 1.2e-9, kGnss); EXPECT(equalsObj(f)); EXPECT(equalsXML(f)); EXPECT(equalsBinary(f)); @@ -345,17 +346,10 @@ TEST(DopplerFactorArm, Serialization) { /* ************************************************************************* */ TEST(DopplerFactorArm, SerializationNavFrame) { - DopplerFactorArm f(0, 1, 2, -1500.0, kLam, kSat1, Point3(-1200, 2400, 800), - kSat1, kLever, Pose3(Rot3::RzRyRx(0.1, 0.4, -0.7), kSat1), - Point3(0.02, -0.05, 0.1), 1.2e-9, kGnss); - EXPECT(equalsObj(f)); - EXPECT(equalsXML(f)); - EXPECT(equalsBinary(f)); -} - -/* ************************************************************************* */ -TEST(ClockDriftFactor, Serialization) { - ClockDriftFactor f(0, 1, 2, 0.2, kGnss); + DopplerFactorArm f(0, 1, 2, 3, -1500.0, kLam, kSat1, + Point3(-1200, 2400, 800), kSat1, kLever, + Pose3(Rot3::RzRyRx(0.1, 0.4, -0.7), kSat1), + Point3(0.02, -0.05, 0.1), 1.0, 1.2e-9, kGnss); EXPECT(equalsObj(f)); EXPECT(equalsXML(f)); EXPECT(equalsBinary(f)); From 9dd84de1ee31e19e12b817f2273ddcdfaeb7928b Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 3 Jul 2026 01:14:02 +0000 Subject: [PATCH 5/8] navigation: trim DopplerFactor doc comments Co-Authored-By: Claude Fable 5 Claude-Session: https://claude.ai/code/session_01RUiPXeNrWr3cmpb2Lsa3HK --- gtsam/navigation/DopplerFactor.cpp | 2 +- gtsam/navigation/DopplerFactor.h | 16 ++++++---------- 2 files changed, 7 insertions(+), 11 deletions(-) diff --git a/gtsam/navigation/DopplerFactor.cpp b/gtsam/navigation/DopplerFactor.cpp index fd859119d0..487289328a 100644 --- a/gtsam/navigation/DopplerFactor.cpp +++ b/gtsam/navigation/DopplerFactor.cpp @@ -36,7 +36,7 @@ DopplerFactor::DopplerFactor(const Key velocityKey, const Key clockBiasPrevKey, gnss::geodist(satellitePosition, receiverPosition, e); los_ = e; - // Earth-rotation (Sagnac) rate term, matching RTKLIB resdop(): + // Earth-rotation (Sagnac) rate term: // (OMGE/c) * (v_s.y*r_r.x + r_s.y*v_r.x - v_s.x*r_r.y - r_s.x*v_r.y) // Split into the v_r-independent offset and the linear coefficient on v_r. const double k = OMGE / C_LIGHT; diff --git a/gtsam/navigation/DopplerFactor.h b/gtsam/navigation/DopplerFactor.h index 1159969181..32140c5755 100644 --- a/gtsam/navigation/DopplerFactor.h +++ b/gtsam/navigation/DopplerFactor.h @@ -43,7 +43,7 @@ namespace gtsam { * - sagnac_rate = (OMGE/c) * (v_s.y*r_r.x + r_s.y*v_r.x * - v_s.x*r_r.y - r_s.x*v_r.y) * is the earth-rotation (Sagnac) rate correction -- the time derivative of - * the Sagnac range term, matching RTKLIB's resdop(), + * the Sagnac range term, * - lambda * Doppler is the measured range rate (m/s); a positive Doppler * (approaching satellite) corresponds to a decreasing range, hence the * leading minus sign. @@ -52,15 +52,11 @@ namespace gtsam { * keeps the state vector identical to the pseudorange-only problem and needs * no extra between-epoch clock factor: the Doppler measurement itself * constrains the clock-bias evolution. The instantaneous drift is - * approximated by its average over [t_{k-1}, t_k], which is the standard - * time-differenced formulation (same structure as TDCP) and is accurate for - * the ~1 s epoch intervals typical of GNSS receivers. + * approximated by its average over [t_{k-1}, t_k]. * * The satellite position/velocity and the line-of-sight are held constant per * factor; only the (second-order) dependence of the line-of-sight on the - * receiver position is neglected, which is standard for Doppler velocity - * estimation. Doppler provides receiver velocity and clock observability - * that is robust through carrier-phase cycle slips and re-convergence. + * receiver position is neglected. * * Keys: [velocity (Vector3, m/s), * receiver clock bias at epoch k-1 (double, s), @@ -177,9 +173,9 @@ struct traits : public Testable {}; * * The line-of-sight e and the Sagnac terms are evaluated at the provided * (nominal) receiver position, exactly as in DopplerFactor -- the second-order - * dependence of the LOS on the pose translation is neglected (standard for - * Doppler velocity estimation), so the pose enters only through its attitude. - * With omega = 0 the factor reduces to DopplerFactor evaluated at `velocity`. + * dependence of the LOS on the pose translation is neglected, so the pose + * enters only through its attitude. With omega = 0 the factor reduces to + * DopplerFactor evaluated at `velocity`. * * When the optional ecef_T_nav transform is provided, the pose key is a local * navigation-frame pose (e.g. ENU) and ecef_R_body = ecef_R_nav * nav_R_body; From 8175b0519950f481fa9d5a7e905f1e4f6b17d727 Mon Sep 17 00:00:00 2001 From: inuex35 Date: Fri, 3 Jul 2026 13:56:03 +0900 Subject: [PATCH 6/8] navigation: fix DopplerFactor doc comments, print, and add Arm dt test 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) --- gtsam/navigation/DopplerFactor.cpp | 4 +++- gtsam/navigation/DopplerFactor.h | 18 ++++++++++++------ gtsam/navigation/tests/testDopplerFactor.cpp | 13 +++++++++++++ 3 files changed, 28 insertions(+), 7 deletions(-) diff --git a/gtsam/navigation/DopplerFactor.cpp b/gtsam/navigation/DopplerFactor.cpp index 487289328a..47c5296c88 100644 --- a/gtsam/navigation/DopplerFactor.cpp +++ b/gtsam/navigation/DopplerFactor.cpp @@ -1,7 +1,7 @@ /** * @file DopplerFactor.cpp * @brief Implementation of the GNSS Doppler (range-rate) factor - * @date June 17, 2026 + * @date July 2026 **/ #include "DopplerFactor.h" @@ -172,9 +172,11 @@ void DopplerFactorArm::print(const std::string& s, gtsam::print(Vector(los_), "line-of-sight (rcv->sat): "); gtsam::print(satClkDrift_, "sat clock drift (s/s): "); gtsam::print(dt_, "epoch interval dt (s): "); + gtsam::print(Vector(velSagnac_), "Sagnac rate coeff (1/s): "); gtsam::print(sagnacOffset_, "Sagnac rate offset (m/s): "); gtsam::print(Vector(arm_.b), "lever arm (body m): "); gtsam::print(Vector(leverVel_), "lever velocity omega x b (m/s): "); + if (arm_.ecef_T_nav) arm_.ecef_T_nav->print("ecef_T_nav: "); } //*************************************************************************** diff --git a/gtsam/navigation/DopplerFactor.h b/gtsam/navigation/DopplerFactor.h index 32140c5755..a9486f750d 100644 --- a/gtsam/navigation/DopplerFactor.h +++ b/gtsam/navigation/DopplerFactor.h @@ -1,7 +1,7 @@ /** * @file DopplerFactor.h * @brief Header file for the GNSS Doppler (range-rate) factor - * @date June 17, 2026 + * @date July 2026 **/ #pragma once @@ -42,8 +42,7 @@ namespace gtsam { * - ddt_s is the satellite clock drift (s/s), * - sagnac_rate = (OMGE/c) * (v_s.y*r_r.x + r_s.y*v_r.x * - v_s.x*r_r.y - r_s.x*v_r.y) - * is the earth-rotation (Sagnac) rate correction -- the time derivative of - * the Sagnac range term, + * is the earth-rotation (Sagnac) correction to the range rate, * - lambda * Doppler is the measured range rate (m/s); a positive Doppler * (approaching satellite) corresponds to a decreasing range, hence the * leading minus sign. @@ -54,9 +53,16 @@ namespace gtsam { * constrains the clock-bias evolution. The instantaneous drift is * approximated by its average over [t_{k-1}, t_k]. * - * The satellite position/velocity and the line-of-sight are held constant per - * factor; only the (second-order) dependence of the line-of-sight on the - * receiver position is neglected. + * The satellite position/velocity and the receiver position are inputs held + * constant per factor; the line-of-sight is computed once from them, so it does + * not depend on any state and enters no Jacobian. + * + * The two clock-bias keys must be distinct states at adjacent epochs: passing + * the same key for both forces (dt_r(k) - dt_r(k-1)) = 0, i.e. zero drift. The + * first epoch has no k-1 bias, so a Doppler factor is added only from the + * second epoch on. With per-constellation receiver clock biases the drift is + * common to all systems (the inter-system biases are constant), so every + * Doppler factor keys on one consistent receiver clock-bias series. * * Keys: [velocity (Vector3, m/s), * receiver clock bias at epoch k-1 (double, s), diff --git a/gtsam/navigation/tests/testDopplerFactor.cpp b/gtsam/navigation/tests/testDopplerFactor.cpp index 95a6b5769d..5bf4bf5a78 100644 --- a/gtsam/navigation/tests/testDopplerFactor.cpp +++ b/gtsam/navigation/tests/testDopplerFactor.cpp @@ -209,6 +209,19 @@ TEST(TestDopplerFactorArm, equals) { f1.print("dopplerArm "); } +// ************************************************************************* +TEST(TestDopplerFactorArm, InvalidDtThrows) { + const Point3 satVel(100, 200, 300), lever(0.5, -0.3, 1.0), omega(0.02, 0, 0.1); + CHECK_EXCEPTION( + DopplerFactorArm(0, 1, 2, 3, 10.0, kLambdaL1, sample::kSatPos, satVel, + sample::kReceiverPos, lever, omega, 0.0), + std::invalid_argument); + CHECK_EXCEPTION( + DopplerFactorArm(0, 1, 2, 3, 10.0, kLambdaL1, sample::kSatPos, satVel, + sample::kReceiverPos, lever, omega, -1.0), + std::invalid_argument); +} + // ************************************************************************* int main() { TestResult tr; From c1bb0f6d7b445d4702ebb7b0bc3dea19d846c97e Mon Sep 17 00:00:00 2001 From: Claude Date: Tue, 7 Jul 2026 01:08:41 +0000 Subject: [PATCH 7/8] navigation: tighten DopplerFactor clock-bias key usage comment Condense the paragraph on clock-bias key requirements from six lines to four while keeping the three usage rules: distinct keys at adjacent epochs, no Doppler factor at the first epoch, and a single receiver clock-bias series across constellations. Co-Authored-By: Claude Fable 5 Claude-Session: https://claude.ai/code/session_017YzbHF4mwqJ5EVKGsF6n96 --- gtsam/navigation/DopplerFactor.h | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/gtsam/navigation/DopplerFactor.h b/gtsam/navigation/DopplerFactor.h index a9486f750d..91a182a06f 100644 --- a/gtsam/navigation/DopplerFactor.h +++ b/gtsam/navigation/DopplerFactor.h @@ -57,12 +57,10 @@ namespace gtsam { * constant per factor; the line-of-sight is computed once from them, so it does * not depend on any state and enters no Jacobian. * - * The two clock-bias keys must be distinct states at adjacent epochs: passing - * the same key for both forces (dt_r(k) - dt_r(k-1)) = 0, i.e. zero drift. The - * first epoch has no k-1 bias, so a Doppler factor is added only from the - * second epoch on. With per-constellation receiver clock biases the drift is - * common to all systems (the inter-system biases are constant), so every - * Doppler factor keys on one consistent receiver clock-bias series. + * The two clock-bias keys must be distinct states at adjacent epochs (the + * same key twice forces zero drift), so Doppler factors start at the second + * epoch. With per-constellation clock biases the drift is common to all + * systems, so key every Doppler factor on a single clock-bias series. * * Keys: [velocity (Vector3, m/s), * receiver clock bias at epoch k-1 (double, s), From 9eca61375180b7a3e25c132953bc94b73c611862 Mon Sep 17 00:00:00 2001 From: Claude Date: Tue, 7 Jul 2026 01:12:27 +0000 Subject: [PATCH 8/8] navigation: condense DopplerFactor and DopplerFactorArm class comments Fold the symbol-by-symbol bullet list into one sentence, drop the restated drift derivation and lever-arm motivation, and keep the error equations, frame conventions, and key ordering. Co-Authored-By: Claude Fable 5 Claude-Session: https://claude.ai/code/session_017YzbHF4mwqJ5EVKGsF6n96 --- gtsam/navigation/DopplerFactor.h | 76 +++++++++----------------------- 1 file changed, 21 insertions(+), 55 deletions(-) diff --git a/gtsam/navigation/DopplerFactor.h b/gtsam/navigation/DopplerFactor.h index 91a182a06f..64c801d0fc 100644 --- a/gtsam/navigation/DopplerFactor.h +++ b/gtsam/navigation/DopplerFactor.h @@ -18,44 +18,23 @@ namespace gtsam { /** * GNSS Doppler (range-rate) factor. * - * Relates a measured Doppler observation to the receiver velocity and the - * receiver clock, reusing the line-of-sight unit vector from gnss::geodist. - * The receiver clock drift is not a separate state: it is expressed as the - * time difference of the two adjacent receiver clock-bias states already - * estimated by the pseudorange / carrier-phase factors, - * - * drift ~= (dt_r(k) - dt_r(k-1)) / dt - * - * so the factor reads + * Constrains the receiver ECEF velocity and the receiver clock drift, where + * the drift is not a separate state but the time difference of the clock-bias + * states at adjacent epochs: * * error = e . (v_s - v_r) * + c * ((dt_r(k) - dt_r(k-1)) / dt - ddt_s) * + sagnac_rate * - (-lambda * Doppler) * - * where - * - e is the unit line-of-sight vector from receiver to satellite, - * - v_s, v_r are the satellite and receiver ECEF velocities (m/s), - * - dt_r(k-1), dt_r(k) are the receiver clock biases (s) at the previous - * and current epochs -- the estimated states, - * - dt is the epoch interval t_k - t_{k-1} (s), - * - ddt_s is the satellite clock drift (s/s), - * - sagnac_rate = (OMGE/c) * (v_s.y*r_r.x + r_s.y*v_r.x - * - v_s.x*r_r.y - r_s.x*v_r.y) - * is the earth-rotation (Sagnac) correction to the range rate, - * - lambda * Doppler is the measured range rate (m/s); a positive Doppler - * (approaching satellite) corresponds to a decreasing range, hence the - * leading minus sign. - * - * Keying on the two clock biases (instead of a dedicated clock-drift state) - * keeps the state vector identical to the pseudorange-only problem and needs - * no extra between-epoch clock factor: the Doppler measurement itself - * constrains the clock-bias evolution. The instantaneous drift is - * approximated by its average over [t_{k-1}, t_k]. + * with e the unit line-of-sight vector (receiver -> satellite), v_s/v_r the + * satellite/receiver ECEF velocities (m/s), dt_r the receiver clock biases + * (s), dt = t_k - t_{k-1} (s), ddt_s the satellite clock drift (s/s), and + * sagnac_rate the earth-rotation correction to the range rate. The measured + * range rate is -lambda * Doppler (positive Doppler = closing range). * - * The satellite position/velocity and the receiver position are inputs held - * constant per factor; the line-of-sight is computed once from them, so it does - * not depend on any state and enters no Jacobian. + * Satellite position/velocity and receiver position are constants per factor: + * the line-of-sight is computed once from them and enters no Jacobian. * * The two clock-bias keys must be distinct states at adjacent epochs (the * same key twice forces zero drift), so Doppler factors start at the second @@ -155,35 +134,22 @@ template <> struct traits : public Testable {}; /** - * GNSS Doppler factor with lever-arm correction. + * DopplerFactor with a kinematic lever-arm correction, keyed on a body Pose3. * - * Like DopplerFactor, but keys on a body Pose3 so the antenna's lever arm can - * be accounted for. Unlike the pseudorange/carrier lever-arm factors (which - * correct the antenna *position*), the dominant lever-arm effect on a Doppler - * (range-rate) observation is *kinematic*: when the body rotates at angular - * rate omega, the antenna moves relative to the body origin, so + * When the body rotates at angular rate omega, the antenna moves relative to + * the body origin, so the range-rate error uses * * v_antenna = v_body + ecef_R_body * (omega x leverArm) * - * where omega is the (measured) body-frame angular velocity and leverArm is the - * body-frame antenna offset. The range-rate error then uses v_antenna: - * - * error = e . (v_s - v_antenna) - * + c * ((dt_r(k) - dt_r(k-1)) / dt - ddt_s) - * + sagnac_rate - (-lambda*Doppler) - * - * with the receiver clock drift expressed through the two adjacent clock-bias - * states, exactly as in DopplerFactor. - * - * The line-of-sight e and the Sagnac terms are evaluated at the provided - * (nominal) receiver position, exactly as in DopplerFactor -- the second-order - * dependence of the LOS on the pose translation is neglected, so the pose - * enters only through its attitude. With omega = 0 the factor reduces to - * DopplerFactor evaluated at `velocity`. + * in place of v_r, with omega the measured body-frame angular velocity and + * leverArm the body-frame antenna offset. Everything else (clock-drift term, + * line-of-sight and Sagnac terms at the nominal receiver position) matches + * DopplerFactor; the pose enters only through its attitude, and with + * omega = 0 the factor reduces to DopplerFactor. * - * When the optional ecef_T_nav transform is provided, the pose key is a local - * navigation-frame pose (e.g. ENU) and ecef_R_body = ecef_R_nav * nav_R_body; - * `velocity` is still the receiver ECEF velocity. + * With the optional ecef_T_nav transform the pose key is a local nav-frame + * pose (e.g. ENU) and ecef_R_body = ecef_R_nav * nav_R_body; `velocity` is + * still the receiver ECEF velocity. * * Keys: [pose (Pose3), velocity (Vector3, ECEF m/s), * clock bias k-1 (double, s), clock bias k (double, s)].