Merge pull request #2447 from inuex35/feature/PseudorangeFactorLever

Add PseudorangeFactorArm and DifferentialPseudorangeFactorArm

Author: dellaert

gtsam/navigation/PseudorangeFactor.cpp

@@ -7,6 +7,8 @@
 
 #include "PseudorangeFactor.h"
 
+#include <limits>
+
 namespace {
 
 /// Speed of light in a vacuum (m/s):
@@ -134,4 +136,145 @@ Vector DifferentialPseudorangeFactor::evaluateError(
 
   return Vector1(error);
 }
+//***************************************************************************
+PseudorangeFactorArm::PseudorangeFactorArm(
+    const Key ecefTbodyKey, const Key receiverClockBiasKey,
+    const double measuredPseudorange, const Point3& satellitePosition,
+    const Point3& leverArm, const double satelliteClockBias,
+    const SharedNoiseModel& model)
+    : Base(model, ecefTbodyKey, receiverClockBiasKey),
+      PseudorangeBase{measuredPseudorange, satellitePosition,
+                      satelliteClockBias},
+      bL_(leverArm) {}
+
+//***************************************************************************
+void PseudorangeFactorArm::print(const std::string& s,
+                                  const KeyFormatter& keyFormatter) const {
+  Base::print(s, keyFormatter);
+  gtsam::print(pseudorange_, "pseudorange (m): ");
+  gtsam::print(Vector(satPos_), "sat position (ECEF meters): ");
+  gtsam::print(satClkBias_, "sat clock bias (s): ");
+  gtsam::print(Vector(bL_), "lever arm (body frame meters): ");
+}
+
+//***************************************************************************
+bool PseudorangeFactorArm::equals(const NonlinearFactor& expected,
+                                   double tol) const {
+  const This* e = dynamic_cast<const This*>(&expected);
+  return e != nullptr && Base::equals(*e, tol) &&
+         traits<double>::Equals(pseudorange_, e->pseudorange_, tol) &&
+         traits<Point3>::Equals(satPos_, e->satPos_, tol) &&
+         traits<double>::Equals(satClkBias_, e->satClkBias_, tol) &&
+         traits<Point3>::Equals(bL_, e->bL_, tol);
+}
+
+//***************************************************************************
+Vector PseudorangeFactorArm::evaluateError(
+    const Pose3& ecef_T_body, const double& receiverClockBias,
+    OptionalMatrixType H_ecef_T_body,
+    OptionalMatrixType HreceiverClockBias) const {
+  // Compute antenna position in the ECEF frame:
+  const Matrix3 ecef_R_body = ecef_T_body.rotation().matrix();
+  const Point3 antennaPos = ecef_T_body.translation() + ecef_R_body * bL_;
+
+  // Apply pseudorange equation: rho = range + c*[dt_u - dt^s]
+  const Vector3 position_difference = antennaPos - satPos_;
+  const double range = position_difference.norm();
+  const double rho = range + CLIGHT * (receiverClockBias - satClkBias_);
+  const double error = rho - pseudorange_;
+
+  // Compute associated derivatives:
+  if (H_ecef_T_body) {
+    H_ecef_T_body->resize(1, 6);
+    if (range < std::numeric_limits<double>::epsilon()) {
+      H_ecef_T_body->setZero();
+    } else {
+      // u = unit vector from satellite to antenna
+      const Matrix u = (position_difference / range).transpose();  // 1x3
+      H_ecef_T_body->block<1, 3>(0, 0) =
+          u * (-ecef_R_body * skewSymmetric(bL_));
+      H_ecef_T_body->block<1, 3>(0, 3) = u * ecef_R_body;
+    }
+  }
+
+  if (HreceiverClockBias) {
+    *HreceiverClockBias = I_1x1 * CLIGHT;
+  }
+
+  return Vector1(error);
+}
+
+//***************************************************************************
+DifferentialPseudorangeFactorArm::DifferentialPseudorangeFactorArm(
+    const Key ecefTbodyKey, const Key receiverClockBiasKey,
+    const Key differentialCorrectionKey, const double measuredPseudorange,
+    const Point3& satellitePosition, const Point3& leverArm,
+    const double satelliteClockBias, const SharedNoiseModel& model)
+    : Base(model, ecefTbodyKey, receiverClockBiasKey, differentialCorrectionKey),
+      PseudorangeBase{measuredPseudorange, satellitePosition,
+                      satelliteClockBias},
+      bL_(leverArm) {}
+
+//***************************************************************************
+void DifferentialPseudorangeFactorArm::print(
+    const std::string& s, const KeyFormatter& keyFormatter) const {
+  Base::print(s, keyFormatter);
+  gtsam::print(pseudorange_, "pseudorange (m): ");
+  gtsam::print(Vector(satPos_), "sat position (ECEF meters): ");
+  gtsam::print(satClkBias_, "sat clock bias (s): ");
+  gtsam::print(Vector(bL_), "lever arm (body frame meters): ");
+}
+
+//***************************************************************************
+bool DifferentialPseudorangeFactorArm::equals(
+    const NonlinearFactor& expected, double tol) const {
+  const This* e = dynamic_cast<const This*>(&expected);
+  return e != nullptr && Base::equals(*e, tol) &&
+         traits<double>::Equals(pseudorange_, e->pseudorange_, tol) &&
+         traits<Point3>::Equals(satPos_, e->satPos_, tol) &&
+         traits<double>::Equals(satClkBias_, e->satClkBias_, tol) &&
+         traits<Point3>::Equals(bL_, e->bL_, tol);
+}
+
+//***************************************************************************
+Vector DifferentialPseudorangeFactorArm::evaluateError(
+    const Pose3& ecef_T_body, const double& receiverClockBias,
+    const double& differentialCorrection, OptionalMatrixType H_ecef_T_body,
+    OptionalMatrixType HreceiverClockBias,
+    OptionalMatrixType HdifferentialCorrection) const {
+  // Compute antenna position in the ECEF frame:
+  const Matrix3 ecef_R_body = ecef_T_body.rotation().matrix();
+  const Point3 antennaPos = ecef_T_body.translation() + ecef_R_body * bL_;
+
+  // Apply pseudorange equation: rho = range + c*[dt_u - dt^s]
+  const Vector3 position_difference = antennaPos - satPos_;
+  const double range = position_difference.norm();
+  const double rho = range + CLIGHT * (receiverClockBias - satClkBias_);
+  const double error = rho - pseudorange_ - differentialCorrection;
+
+  // Compute associated derivatives:
+  if (H_ecef_T_body) {
+    H_ecef_T_body->resize(1, 6);
+    if (range < std::numeric_limits<double>::epsilon()) {
+      H_ecef_T_body->setZero();
+    } else {
+      // u = unit vector from satellite to antenna
+      const Matrix u = (position_difference / range).transpose();  // 1x3
+      H_ecef_T_body->block<1, 3>(0, 0) =
+          u * (-ecef_R_body * skewSymmetric(bL_));
+      H_ecef_T_body->block<1, 3>(0, 3) = u * ecef_R_body;
+    }
+  }
+
+  if (HreceiverClockBias) {
+    *HreceiverClockBias = I_1x1 * CLIGHT;
+  }
+
+  if (HdifferentialCorrection) {
+    *HdifferentialCorrection = -I_1x1;
+  }
+
+  return Vector1(error);
+}
+
 }  // namespace gtsam

gtsam/navigation/PseudorangeFactor.h

@@ -7,6 +7,7 @@
 #pragma once
 
 #include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Pose3.h>
 #include <gtsam/nonlinear/NoiseModelFactorN.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 
@@ -225,4 +226,210 @@ template <>
 struct traits<DifferentialPseudorangeFactor>
     : public Testable<DifferentialPseudorangeFactor> {};
 
+/**
+ * GNSS pseudorange factor with lever arm correction.
+ *
+ * Like PseudorangeFactor, but uses a Pose3 (position + attitude) as the
+ * receiver state variable, allowing compensation for a lever arm offset
+ * between the body frame origin and the GNSS antenna location.
+ *
+ * The antenna position is computed as:
+ *   antenna_pos = ecef_T_body.translation() + ecef_R_body * bL_
+ *
+ * where bL_ is the lever arm from the body origin to the antenna in the body
+ * frame. The error model is:
+ *   error = ||antenna_pos - satPos|| + c*(dt_u - dt_s) - pseudorange
+ *
+ * @note Both the body pose and satellite position are expected in the ECEF
+ *       frame. Any conversion to/from a local navigation frame (NED/ENU) is
+ *       the caller's responsibility.
+ *
+ * @ingroup navigation
+ */
+class GTSAM_EXPORT PseudorangeFactorArm
+    : public NoiseModelFactorN<Pose3, double>,
+      private PseudorangeBase {
+ private:
+  typedef NoiseModelFactorN<Pose3, double> Base;
+
+  Point3 bL_;  ///< Lever arm from body origin to antenna in body frame.
+
+ public:
+  // Provide access to the Matrix& version of evaluateError:
+  using Base::evaluateError;
+
+  /// shorthand for a smart pointer to a factor
+  typedef std::shared_ptr<PseudorangeFactorArm> shared_ptr;
+
+  /// Typedef to this class
+  typedef PseudorangeFactorArm This;
+
+  /** default constructor - only use for serialization */
+  PseudorangeFactorArm() : PseudorangeBase{0.0, Point3(0, 0, 0), 0.0}, bL_(0, 0, 0) {}
+
+  virtual ~PseudorangeFactorArm() = default;
+
+  /**
+   * Construct a PseudorangeFactorArm.
+   *
+   * @param ecefTbodyKey Receiver gtsam::Pose3 key (body pose in ECEF frame).
+   * @param receiverClockBiasKey Receiver clock bias node.
+   * @param measuredPseudorange Receiver-measured pseudorange in meters.
+   * @param satellitePosition Satellite ECEF position in meters.
+   * @param leverArm Translation from body origin to antenna in body frame.
+   * @param satelliteClockBias Satellite clock bias in seconds.
+   * @param model 1-D pseudorange noise model.
+   */
+  PseudorangeFactorArm(
+      Key ecefTbodyKey, Key receiverClockBiasKey,
+      double measuredPseudorange, const Point3& satellitePosition,
+      const Point3& leverArm, double satelliteClockBias = 0.0,
+      const SharedNoiseModel& model = noiseModel::Unit::Create(1));
+
+  /// @return a deep copy of this factor
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
+    return std::static_pointer_cast<gtsam::NonlinearFactor>(
+        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
+  }
+
+  /// print
+  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
+                                            DefaultKeyFormatter) const override;
+
+  /// equals
+  bool equals(const NonlinearFactor& expected,
+              double tol = 1e-9) const override;
+
+  /// vector of errors
+  Vector evaluateError(const Pose3& ecef_T_body,
+                       const double& receiverClockBias,
+                       OptionalMatrixType H_ecef_T_body,
+                       OptionalMatrixType HreceiverClockBias) const override;
+
+  /// return the lever arm, a position in the body frame
+  inline const Point3& leverArm() const { return bL_; }
+
+ private:
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+  /// Serialization function
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PseudorangeFactorArm::Base);
+    ar& BOOST_SERIALIZATION_NVP(pseudorange_);
+    ar& BOOST_SERIALIZATION_NVP(satPos_);
+    ar& BOOST_SERIALIZATION_NVP(satClkBias_);
+    ar& BOOST_SERIALIZATION_NVP(bL_);
+  }
+#endif
+};
+
+/// traits
+template <>
+struct traits<PseudorangeFactorArm>
+    : public Testable<PseudorangeFactorArm> {};
+
+/**
+ * Differentially-corrected pseudorange factor with lever arm correction.
+ *
+ * Combines the differential correction model of DifferentialPseudorangeFactor
+ * with the lever arm compensation of PseudorangeFactorArm. Uses a Pose3
+ * (position + attitude) as the receiver state variable.
+ *
+ * The error model is:
+ *   error = ||antenna_pos - satPos|| + c*(dt_u - dt_s) - pseudorange - correction
+ *
+ * @note Both the body pose and satellite position are expected in the ECEF
+ *       frame. Any conversion to/from a local navigation frame (NED/ENU) is
+ *       the caller's responsibility.
+ *
+ * @ingroup navigation
+ */
+class GTSAM_EXPORT DifferentialPseudorangeFactorArm
+    : public NoiseModelFactorN<Pose3, double, double>,
+      private PseudorangeBase {
+ private:
+  typedef NoiseModelFactorN<Pose3, double, double> Base;
+
+  Point3 bL_;  ///< Lever arm from body origin to antenna in body frame.
+
+ public:
+  // Provide access to the Matrix& version of evaluateError:
+  using Base::evaluateError;
+
+  /// shorthand for a smart pointer to a factor
+  typedef std::shared_ptr<DifferentialPseudorangeFactorArm> shared_ptr;
+
+  /// Typedef to this class
+  typedef DifferentialPseudorangeFactorArm This;
+
+  /** default constructor - only use for serialization */
+  DifferentialPseudorangeFactorArm() : PseudorangeBase{0.0, Point3(0, 0, 0), 0.0}, bL_(0, 0, 0) {}
+
+  virtual ~DifferentialPseudorangeFactorArm() = default;
+
+  /**
+   * Construct a DifferentialPseudorangeFactorArm.
+   *
+   * @param ecefTbodyKey Receiver gtsam::Pose3 key (body pose in ECEF frame).
+   * @param receiverClockBiasKey Receiver clock bias node.
+   * @param differentialCorrectionKey Differential correction node.
+   * @param measuredPseudorange Receiver-measured pseudorange in meters.
+   * @param satellitePosition Satellite ECEF position in meters.
+   * @param leverArm Translation from body origin to antenna in body frame.
+   * @param satelliteClockBias Satellite clock bias in seconds.
+   * @param model 1-D pseudorange noise model.
+   */
+  DifferentialPseudorangeFactorArm(
+      Key ecefTbodyKey, Key receiverClockBiasKey, Key differentialCorrectionKey,
+      double measuredPseudorange, const Point3& satellitePosition,
+      const Point3& leverArm, double satelliteClockBias = 0.0,
+      const SharedNoiseModel& model = noiseModel::Unit::Create(1));
+
+  /// @return a deep copy of this factor
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
+    return std::static_pointer_cast<gtsam::NonlinearFactor>(
+        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
+  }
+
+  /// print
+  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
+                                            DefaultKeyFormatter) const override;
+
+  /// equals
+  bool equals(const NonlinearFactor& expected,
+              double tol = 1e-9) const override;
+
+  /// vector of errors
+  Vector evaluateError(const Pose3& ecef_T_body,
+                       const double& receiverClockBias,
+                       const double& differentialCorrection,
+                       OptionalMatrixType H_ecef_T_body,
+                       OptionalMatrixType HreceiverClockBias,
+                       OptionalMatrixType HdifferentialCorrection) const override;
+
+  /// return the lever arm, a position in the body frame
+  inline const Point3& leverArm() const { return bL_; }
+
+ private:
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+  /// Serialization function
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(
+        DifferentialPseudorangeFactorArm::Base);
+    ar& BOOST_SERIALIZATION_NVP(pseudorange_);
+    ar& BOOST_SERIALIZATION_NVP(satPos_);
+    ar& BOOST_SERIALIZATION_NVP(satClkBias_);
+    ar& BOOST_SERIALIZATION_NVP(bL_);
+  }
+#endif
+};
+
+/// traits
+template <>
+struct traits<DifferentialPseudorangeFactorArm>
+    : public Testable<DifferentialPseudorangeFactorArm> {};
+
 }  // namespace gtsam