DopplerFactor.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
 
 * See LICENSE for the license information
 
 * -------------------------------------------------------------------------- */

#pragma once
 
#include <gtsam/base/std_optional_serialization.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/navigation/GnssCommon.h>
#include <gtsam/nonlinear/NoiseModelFactorN.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
 
#include <string>
 
namespace gtsam {

class GTSAM_EXPORT DopplerFactor
    : public NoiseModelFactorT<Vector1, Vector3, double, double> {
 private:
  typedef NoiseModelFactorT<Vector1, Vector3, double, double> Base;
 
  double measRangeRate_ = 0.0;        
  Point3 satVel_{0, 0, 0};            
  Point3 los_{0, 0, 0};              
  double satClkDrift_ = 0.0;          
  double dt_ = 1.0;                   
  Point3 velSagnac_{0, 0, 0};         
  double sagnacOffset_ = 0.0;         
 
 public:
  using Base::evaluateError;
  typedef std::shared_ptr<DopplerFactor> shared_ptr;
  typedef DopplerFactor This;

  DopplerFactor() = default;
  virtual ~DopplerFactor() = default;

  DopplerFactor(Key velocityKey, Key clockBiasPrevKey, Key clockBiasCurrKey,
                double measuredDoppler, double wavelength,
                const Point3& satellitePosition,
                const Point3& satelliteVelocity, const Point3& receiverPosition,
                double dt, 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>(
        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;

  Vector1 evaluateError(const Vector3& velocity, const double& clockBiasPrev,
                        const double& clockBiasCurr,
                        OptionalMatrixType Hvelocity,
                        OptionalMatrixType HclockBiasPrev,
                        OptionalMatrixType HclockBiasCurr) const override;

  inline double measuredRangeRate() const { return measRangeRate_; }
  inline const Point3& lineOfSight() const { return los_; }
  inline double dt() const { return dt_; }
 
 private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
  friend class boost::serialization::access;
  template <class ARCHIVE>
  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(dt_);
    ar& BOOST_SERIALIZATION_NVP(velSagnac_);
    ar& BOOST_SERIALIZATION_NVP(sagnacOffset_);
  }
#endif
};

template <>
struct traits<DopplerFactor> : public Testable<DopplerFactor> {};

class GTSAM_EXPORT DopplerFactorArm
    : public NoiseModelFactorN<Pose3, Vector3, double, double> {
 private:
  typedef NoiseModelFactorN<Pose3, Vector3, double, double> Base;
 
  double measRangeRate_ = 0.0;  
  Point3 satVel_{0, 0, 0};      
  Point3 los_{0, 0, 0};        
  double satClkDrift_ = 0.0;    
  double dt_ = 1.0;             
  Point3 velSagnac_{0, 0, 0};   
  double sagnacOffset_ = 0.0;   
  gnss::LeverArm arm_;          
  Point3 leverVel_{0, 0, 0};    
 
 public:
  using Base::evaluateError;
  typedef std::shared_ptr<DopplerFactorArm> shared_ptr;
  typedef DopplerFactorArm This;

  DopplerFactorArm() = default;
  virtual ~DopplerFactorArm() = default;

  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, double dt,
                   double satelliteClockDrift = 0.0,
                   const SharedNoiseModel& model = noiseModel::Unit::Create(1));

  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 dt, 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>(
        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& clockBiasPrev, const double& clockBiasCurr,
                       OptionalMatrixType Hpose, OptionalMatrixType Hvelocity,
                       OptionalMatrixType HclockBiasPrev,
                       OptionalMatrixType HclockBiasCurr) const override;

  inline double measuredRangeRate() const { return measRangeRate_; }
  inline const Point3& lineOfSight() const { return los_; }
  inline double dt() const { return dt_; }
  inline const Point3& leverArm() const { return arm_.b; }
  inline const std::optional<Pose3>& ecefTnav() const {
    return arm_.ecef_T_nav;
  }
 
 private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
  friend class boost::serialization::access;
  template <class ARCHIVE>
  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(dt_);
    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
};

template <>
struct traits<DopplerFactorArm> : public Testable<DopplerFactorArm> {};
 
}  // namespace gtsam