Skip to content

Commit 3c2f54c

Browse files
dellaertdellaert
authored
Merge pull request #2575 from inuex35/feature/ppp-undifferenced-factors
navigation: add undifferenced (PPP) pseudorange and carrier-phase factors
2 parents f1a6956 + 83c149e commit 3c2f54c

10 files changed

Lines changed: 1763 additions & 0 deletions

‎gtsam/navigation/CarrierPhaseFactor.cpp‎

Lines changed: 162 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -72,6 +72,168 @@ Vector CarrierPhaseFactor::evaluateError(
7272
return Vector1(error);
7373
}
7474

75+
//***************************************************************************
76+
UndifferencedCarrierPhaseFactor::UndifferencedCarrierPhaseFactor(
77+
const Key receiverPositionKey, const Key receiverClockBiasKey,
78+
const Key tropoZenithWetKey, const Key slantIonoKey, const Key ambiguityKey,
79+
const double measuredCarrierPhaseMeters, const Point3& satellitePosition,
80+
const double tropoWetMapping, const double ionoCoefficient,
81+
const double lambda, const double satelliteClockBias,
82+
const SharedNoiseModel& model)
83+
: Base(model, receiverPositionKey, receiverClockBiasKey, tropoZenithWetKey,
84+
slantIonoKey, ambiguityKey),
85+
CarrierPhaseBase{measuredCarrierPhaseMeters, satellitePosition,
86+
satelliteClockBias},
87+
tropoMap_(tropoWetMapping),
88+
ionoCoeff_(ionoCoefficient),
89+
lambda_(lambda) {}
90+
91+
//***************************************************************************
92+
void UndifferencedCarrierPhaseFactor::print(
93+
const std::string& s, const KeyFormatter& keyFormatter) const {
94+
Base::print(s, keyFormatter);
95+
gtsam::print(measurement_, "carrier phase (m): ");
96+
gtsam::print(Vector(satPos_), "sat position (ECEF meters): ");
97+
gtsam::print(satClkBias_, "sat clock bias (s): ");
98+
gtsam::print(tropoMap_, "tropo wet mapping: ");
99+
gtsam::print(ionoCoeff_, "iono coefficient: ");
100+
gtsam::print(lambda_, "wavelength (m): ");
101+
}
102+
103+
//***************************************************************************
104+
bool UndifferencedCarrierPhaseFactor::equals(const NonlinearFactor& expected,
105+
double tol) const {
106+
const This* e = dynamic_cast<const This*>(&expected);
107+
return e != nullptr && Base::equals(*e, tol) &&
108+
traits<double>::Equals(measurement_, e->measurement_, tol) &&
109+
traits<Point3>::Equals(satPos_, e->satPos_, tol) &&
110+
traits<double>::Equals(satClkBias_, e->satClkBias_, tol) &&
111+
traits<double>::Equals(tropoMap_, e->tropoMap_, tol) &&
112+
traits<double>::Equals(ionoCoeff_, e->ionoCoeff_, tol) &&
113+
traits<double>::Equals(lambda_, e->lambda_, tol);
114+
}
115+
116+
//***************************************************************************
117+
Vector UndifferencedCarrierPhaseFactor::evaluateError(
118+
const Point3& receiverPosition, const double& receiverClockBias,
119+
const double& tropoZenithWet, const double& slantIono,
120+
const double& ambiguity, OptionalMatrixType HreceiverPos,
121+
OptionalMatrixType HreceiverClockBias, OptionalMatrixType HtropoZenithWet,
122+
OptionalMatrixType HslantIono, OptionalMatrixType Hambiguity) const {
123+
// Undifferenced PPP carrier phase model (iono advanced, +lambda*N):
124+
// rho = geodist + c*(dt_u - dt_s) + m_w*ZTD - mu_f*I_slant + lambda*N
125+
Point3 e;
126+
Matrix13 H_geo;
127+
const double range = gnss::geodist(satPos_, receiverPosition, e,
128+
HreceiverPos ? &H_geo : nullptr);
129+
const double rho = range + C_LIGHT * (receiverClockBias - satClkBias_) +
130+
tropoMap_ * tropoZenithWet - ionoCoeff_ * slantIono +
131+
lambda_ * ambiguity;
132+
const double error = rho - measurement_;
133+
134+
if (HreceiverPos) *HreceiverPos = H_geo;
135+
if (HreceiverClockBias) *HreceiverClockBias = I_1x1 * C_LIGHT;
136+
if (HtropoZenithWet) *HtropoZenithWet = I_1x1 * tropoMap_;
137+
if (HslantIono) *HslantIono = I_1x1 * (-ionoCoeff_);
138+
if (Hambiguity) *Hambiguity = I_1x1 * lambda_;
139+
140+
return Vector1(error);
141+
}
142+
143+
//***************************************************************************
144+
UndifferencedCarrierPhaseFactorArm::UndifferencedCarrierPhaseFactorArm(
145+
const Key poseKey, const Key receiverClockBiasKey,
146+
const Key tropoZenithWetKey, const Key slantIonoKey, const Key ambiguityKey,
147+
const double measuredCarrierPhaseMeters, const Point3& satellitePosition,
148+
const Point3& leverArm, const double tropoWetMapping,
149+
const double ionoCoefficient, const double lambda,
150+
const double satelliteClockBias, const SharedNoiseModel& model)
151+
: Base(model, poseKey, receiverClockBiasKey, tropoZenithWetKey, slantIonoKey,
152+
ambiguityKey),
153+
CarrierPhaseBase{measuredCarrierPhaseMeters, satellitePosition,
154+
satelliteClockBias},
155+
arm_(leverArm),
156+
tropoMap_(tropoWetMapping),
157+
ionoCoeff_(ionoCoefficient),
158+
lambda_(lambda) {}
159+
160+
//***************************************************************************
161+
UndifferencedCarrierPhaseFactorArm::UndifferencedCarrierPhaseFactorArm(
162+
const Key poseKey, const Key receiverClockBiasKey,
163+
const Key tropoZenithWetKey, const Key slantIonoKey, const Key ambiguityKey,
164+
const double measuredCarrierPhaseMeters, const Point3& satellitePosition,
165+
const Point3& leverArm, const Pose3& ecef_T_nav,
166+
const double tropoWetMapping, const double ionoCoefficient,
167+
const double lambda, const double satelliteClockBias,
168+
const SharedNoiseModel& model)
169+
: Base(model, poseKey, receiverClockBiasKey, tropoZenithWetKey, slantIonoKey,
170+
ambiguityKey),
171+
CarrierPhaseBase{measuredCarrierPhaseMeters, satellitePosition,
172+
satelliteClockBias},
173+
arm_(leverArm, ecef_T_nav),
174+
tropoMap_(tropoWetMapping),
175+
ionoCoeff_(ionoCoefficient),
176+
lambda_(lambda) {}
177+
178+
//***************************************************************************
179+
void UndifferencedCarrierPhaseFactorArm::print(
180+
const std::string& s, const KeyFormatter& keyFormatter) const {
181+
Base::print(s, keyFormatter);
182+
gtsam::print(measurement_, "carrier phase (m): ");
183+
gtsam::print(Vector(satPos_), "sat position (ECEF meters): ");
184+
gtsam::print(satClkBias_, "sat clock bias (s): ");
185+
gtsam::print(Vector(arm_.b), "lever arm (body frame meters): ");
186+
gtsam::print(tropoMap_, "tropo wet mapping: ");
187+
gtsam::print(ionoCoeff_, "iono coefficient: ");
188+
gtsam::print(lambda_, "wavelength (m): ");
189+
if (arm_.ecef_T_nav) {
190+
arm_.ecef_T_nav->print("ecef_T_nav:\n");
191+
}
192+
}
193+
194+
//***************************************************************************
195+
bool UndifferencedCarrierPhaseFactorArm::equals(const NonlinearFactor& expected,
196+
double tol) const {
197+
const This* e = dynamic_cast<const This*>(&expected);
198+
return e != nullptr && Base::equals(*e, tol) &&
199+
traits<double>::Equals(measurement_, e->measurement_, tol) &&
200+
traits<Point3>::Equals(satPos_, e->satPos_, tol) &&
201+
traits<double>::Equals(satClkBias_, e->satClkBias_, tol) &&
202+
arm_.equals(e->arm_, tol) &&
203+
traits<double>::Equals(tropoMap_, e->tropoMap_, tol) &&
204+
traits<double>::Equals(ionoCoeff_, e->ionoCoeff_, tol) &&
205+
traits<double>::Equals(lambda_, e->lambda_, tol);
206+
}
207+
208+
//***************************************************************************
209+
Vector UndifferencedCarrierPhaseFactorArm::evaluateError(
210+
const Pose3& pose, const double& receiverClockBias,
211+
const double& tropoZenithWet, const double& slantIono,
212+
const double& ambiguity, OptionalMatrixType H_pose,
213+
OptionalMatrixType HreceiverClockBias, OptionalMatrixType HtropoZenithWet,
214+
OptionalMatrixType HslantIono, OptionalMatrixType Hambiguity) const {
215+
gnss::LeverArm::PoseFrame frame;
216+
const Point3 antennaPos =
217+
arm_.antennaPosition(pose, H_pose ? &frame : nullptr);
218+
219+
Point3 e;
220+
Matrix13 H_antenna;
221+
const double range =
222+
gnss::geodist(satPos_, antennaPos, e, H_pose ? &H_antenna : nullptr);
223+
const double rho = range + C_LIGHT * (receiverClockBias - satClkBias_) +
224+
tropoMap_ * tropoZenithWet - ionoCoeff_ * slantIono +
225+
lambda_ * ambiguity;
226+
const double error = rho - measurement_;
227+
228+
if (H_pose) *H_pose = arm_.antennaPoseJacobian(H_antenna, frame);
229+
if (HreceiverClockBias) *HreceiverClockBias = I_1x1 * C_LIGHT;
230+
if (HtropoZenithWet) *HtropoZenithWet = I_1x1 * tropoMap_;
231+
if (HslantIono) *HslantIono = I_1x1 * (-ionoCoeff_);
232+
if (Hambiguity) *Hambiguity = I_1x1 * lambda_;
233+
234+
return Vector1(error);
235+
}
236+
75237
//***************************************************************************
76238
CarrierPhaseFactorArm::CarrierPhaseFactorArm(
77239
const Key poseKey, const Key receiverClockBiasKey, const Key ambiguityKey,

‎gtsam/navigation/CarrierPhaseFactor.h‎

Lines changed: 207 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,11 @@ using CarrierPhaseBase = GnssMeasurementBase;
3636
*
3737
* where dt_u, dt_s are in seconds and ambiguity is in meters.
3838
*
39+
* @note The ambiguity is estimated as a *float* (continuous `double`) in the
40+
* factor graph; GTSAM never optimizes it as an integer. This factor yields
41+
* the float estimate and covariance; integer ambiguity fixing (e.g. LAMBDA)
42+
* is a separate step performed outside the graph.
43+
*
3944
* @ingroup navigation
4045
*/
4146
class GTSAM_EXPORT CarrierPhaseFactor
@@ -112,6 +117,208 @@ class GTSAM_EXPORT CarrierPhaseFactor
112117
template <>
113118
struct traits<CarrierPhaseFactor> : public Testable<CarrierPhaseFactor> {};
114119

120+
/**
121+
* Undifferenced (raw) PPP carrier phase factor.
122+
*
123+
* Models a single raw carrier phase (in meters, with the satellite-side SSR
124+
* corrections including the phase bias already folded into the measurement) and
125+
* carries the receiver clock, zenith wet tropo, slant ionosphere and the
126+
* integer ambiguity as state variables:
127+
*
128+
* error = geodist(sat, rcv) + c*(dt_u - dt_s)
129+
* + m_w * ZTD_wet - mu_f * I_slant + lambda * N - measuredPhase
130+
*
131+
* Compared with the pseudorange model, the ionospheric term is *advanced*
132+
* (negative sign). The ambiguity N is in cycles and lambda is the wavelength
133+
* [m/cycle], so the integer structure is preserved for ambiguity resolution.
134+
* The wet mapping function and iono coefficient are held constant per factor.
135+
*
136+
* @note The ambiguity N is represented as a continuous `double` (in cycles)
137+
* and is estimated as a *float* ambiguity in the factor graph; GTSAM never
138+
* optimizes it as an integer. This factor provides the float estimate and its
139+
* covariance. Integer ambiguity fixing is a separate step performed outside
140+
* the graph, e.g. with the LAMBDA method, using the float estimate and
141+
* covariance.
142+
*
143+
* Keys: [pos, clock, ztd, slant-iono, ambiguity].
144+
*
145+
* @ingroup navigation
146+
*/
147+
class GTSAM_EXPORT UndifferencedCarrierPhaseFactor
148+
: public NoiseModelFactorN<Point3, double, double, double, double>,
149+
private CarrierPhaseBase {
150+
private:
151+
typedef NoiseModelFactorN<Point3, double, double, double, double> Base;
152+
double tropoMap_ = 0.0; ///< Tropospheric wet mapping function (constant).
153+
double ionoCoeff_ = 1.0; ///< Slant-iono coefficient mu_f (constant).
154+
double lambda_ = 1.0; ///< Wavelength [m/cycle] for the ambiguity term.
155+
156+
public:
157+
using Base::evaluateError;
158+
typedef std::shared_ptr<UndifferencedCarrierPhaseFactor> shared_ptr;
159+
typedef UndifferencedCarrierPhaseFactor This;
160+
161+
UndifferencedCarrierPhaseFactor() : CarrierPhaseBase{0.0, Point3(0, 0, 0), 0.0} {}
162+
virtual ~UndifferencedCarrierPhaseFactor() = default;
163+
164+
/**
165+
* @param receiverPositionKey Receiver Point3 ECEF position node.
166+
* @param receiverClockBiasKey Receiver clock bias node [s].
167+
* @param tropoZenithWetKey Tropospheric zenith wet delay node [m].
168+
* @param slantIonoKey Slant ionospheric delay node (this sat) [m].
169+
* @param ambiguityKey Ambiguity node [cycles].
170+
* @param measuredCarrierPhaseMeters SSR-corrected carrier phase [m].
171+
* @param satellitePosition Satellite ECEF position [m].
172+
* @param tropoWetMapping Wet mapping function m_w at predicted elevation.
173+
* @param ionoCoefficient Ionospheric coefficient mu_f (+1 on L1).
174+
* @param lambda Wavelength [m/cycle].
175+
* @param satelliteClockBias Satellite clock bias [s].
176+
* @param model 1-D noise model.
177+
*/
178+
UndifferencedCarrierPhaseFactor(
179+
Key receiverPositionKey, Key receiverClockBiasKey, Key tropoZenithWetKey,
180+
Key slantIonoKey, Key ambiguityKey, double measuredCarrierPhaseMeters,
181+
const Point3& satellitePosition, double tropoWetMapping,
182+
double ionoCoefficient, double lambda, double satelliteClockBias = 0.0,
183+
const SharedNoiseModel& model = noiseModel::Unit::Create(1));
184+
185+
gtsam::NonlinearFactor::shared_ptr clone() const override {
186+
return std::static_pointer_cast<gtsam::NonlinearFactor>(
187+
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
188+
}
189+
190+
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
191+
DefaultKeyFormatter) const override;
192+
bool equals(const NonlinearFactor& expected,
193+
double tol = 1e-9) const override;
194+
195+
Vector evaluateError(const Point3& receiverPosition,
196+
const double& receiverClockBias,
197+
const double& tropoZenithWet, const double& slantIono,
198+
const double& ambiguity, OptionalMatrixType HreceiverPos,
199+
OptionalMatrixType HreceiverClockBias,
200+
OptionalMatrixType HtropoZenithWet,
201+
OptionalMatrixType HslantIono,
202+
OptionalMatrixType Hambiguity) const override;
203+
204+
inline double tropoMapping() const { return tropoMap_; }
205+
inline double ionoCoefficient() const { return ionoCoeff_; }
206+
inline double wavelength() const { return lambda_; }
207+
208+
private:
209+
#if GTSAM_ENABLE_BOOST_SERIALIZATION
210+
friend class boost::serialization::access;
211+
template <class ARCHIVE>
212+
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
213+
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
214+
ar& BOOST_SERIALIZATION_NVP(measurement_);
215+
ar& BOOST_SERIALIZATION_NVP(satPos_);
216+
ar& BOOST_SERIALIZATION_NVP(satClkBias_);
217+
ar& BOOST_SERIALIZATION_NVP(tropoMap_);
218+
ar& BOOST_SERIALIZATION_NVP(ionoCoeff_);
219+
ar& BOOST_SERIALIZATION_NVP(lambda_);
220+
}
221+
#endif
222+
};
223+
224+
/// traits
225+
template <>
226+
struct traits<UndifferencedCarrierPhaseFactor>
227+
: public Testable<UndifferencedCarrierPhaseFactor> {};
228+
229+
/**
230+
* Undifferenced (raw) PPP carrier phase factor with lever-arm correction.
231+
*
232+
* Like UndifferencedCarrierPhaseFactor but keys on a body Pose3 with a lever arm to
233+
* the antenna and an optional ecef_T_nav transform.
234+
* Keys: [pose, clock, ztd, slant-iono, ambiguity].
235+
*
236+
* @ingroup navigation
237+
*/
238+
class GTSAM_EXPORT UndifferencedCarrierPhaseFactorArm
239+
: public NoiseModelFactorN<Pose3, double, double, double, double>,
240+
private CarrierPhaseBase {
241+
private:
242+
typedef NoiseModelFactorN<Pose3, double, double, double, double> Base;
243+
gnss::LeverArm arm_;
244+
double tropoMap_ = 0.0;
245+
double ionoCoeff_ = 1.0;
246+
double lambda_ = 1.0;
247+
248+
public:
249+
using Base::evaluateError;
250+
typedef std::shared_ptr<UndifferencedCarrierPhaseFactorArm> shared_ptr;
251+
typedef UndifferencedCarrierPhaseFactorArm This;
252+
253+
UndifferencedCarrierPhaseFactorArm()
254+
: CarrierPhaseBase{0.0, Point3(0, 0, 0), 0.0} {}
255+
virtual ~UndifferencedCarrierPhaseFactorArm() = default;
256+
257+
/// Construct with an ECEF pose key.
258+
UndifferencedCarrierPhaseFactorArm(
259+
Key poseKey, Key receiverClockBiasKey, Key tropoZenithWetKey,
260+
Key slantIonoKey, Key ambiguityKey, double measuredCarrierPhaseMeters,
261+
const Point3& satellitePosition, const Point3& leverArm,
262+
double tropoWetMapping, double ionoCoefficient, double lambda,
263+
double satelliteClockBias = 0.0,
264+
const SharedNoiseModel& model = noiseModel::Unit::Create(1));
265+
266+
/// Construct with a local nav-frame pose key + ecef_T_nav.
267+
UndifferencedCarrierPhaseFactorArm(
268+
Key poseKey, Key receiverClockBiasKey, Key tropoZenithWetKey,
269+
Key slantIonoKey, Key ambiguityKey, double measuredCarrierPhaseMeters,
270+
const Point3& satellitePosition, const Point3& leverArm,
271+
const Pose3& ecef_T_nav, double tropoWetMapping, double ionoCoefficient,
272+
double lambda, double satelliteClockBias = 0.0,
273+
const SharedNoiseModel& model = noiseModel::Unit::Create(1));
274+
275+
gtsam::NonlinearFactor::shared_ptr clone() const override {
276+
return std::static_pointer_cast<gtsam::NonlinearFactor>(
277+
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
278+
}
279+
280+
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
281+
DefaultKeyFormatter) const override;
282+
bool equals(const NonlinearFactor& expected,
283+
double tol = 1e-9) const override;
284+
285+
Vector evaluateError(const Pose3& pose, const double& receiverClockBias,
286+
const double& tropoZenithWet, const double& slantIono,
287+
const double& ambiguity, OptionalMatrixType H_pose,
288+
OptionalMatrixType HreceiverClockBias,
289+
OptionalMatrixType HtropoZenithWet,
290+
OptionalMatrixType HslantIono,
291+
OptionalMatrixType Hambiguity) const override;
292+
293+
inline const Point3& leverArm() const { return arm_.b; }
294+
inline const std::optional<Pose3>& ecefTnav() const { return arm_.ecef_T_nav; }
295+
inline double tropoMapping() const { return tropoMap_; }
296+
inline double ionoCoefficient() const { return ionoCoeff_; }
297+
inline double wavelength() const { return lambda_; }
298+
299+
private:
300+
#if GTSAM_ENABLE_BOOST_SERIALIZATION
301+
friend class boost::serialization::access;
302+
template <class ARCHIVE>
303+
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
304+
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
305+
ar& BOOST_SERIALIZATION_NVP(measurement_);
306+
ar& BOOST_SERIALIZATION_NVP(satPos_);
307+
ar& BOOST_SERIALIZATION_NVP(satClkBias_);
308+
ar& boost::serialization::make_nvp("bL_", arm_.b);
309+
ar& boost::serialization::make_nvp("ecef_T_nav_", arm_.ecef_T_nav);
310+
ar& BOOST_SERIALIZATION_NVP(tropoMap_);
311+
ar& BOOST_SERIALIZATION_NVP(ionoCoeff_);
312+
ar& BOOST_SERIALIZATION_NVP(lambda_);
313+
}
314+
#endif
315+
};
316+
317+
/// traits
318+
template <>
319+
struct traits<UndifferencedCarrierPhaseFactorArm>
320+
: public Testable<UndifferencedCarrierPhaseFactorArm> {};
321+
115322
/**
116323
* Carrier phase factor with lever arm correction.
117324
*

0 commit comments

Comments
 (0)