GnssCommon.h

Go to the documentation of this file.

#pragma once
 
#include <gtsam/base/Matrix.h>
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/dllexport.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose3.h>
 
#include <optional>
 
namespace gtsam {

struct GnssMeasurementBase {
  double measurement_;
  Point3 satPos_;
  double satClkBias_;
};
 
namespace gnss {

constexpr double C_LIGHT = 299792458.0;

constexpr double OMGE = 7.2921151467e-5;

GTSAM_EXPORT double geodist(const Point3& sat, const Point3& rcv, Point3& e,
                            OptionalJacobian<1, 3> H_rcv = {});

struct GTSAM_EXPORT DoubleDifferenceData {
  double rovRef = 0;      
  double baseRef = 0;     
  double rovTarget = 0;   
  double baseTarget = 0;  
  Point3 satRefRov{0, 0, 0};      
  Point3 satTargetRov{0, 0, 0};   
  Point3 satRefBase{0, 0, 0};     
  Point3 satTargetBase{0, 0, 0};  
  Point3 basePos{0, 0, 0};        

  double observed() const {
    return (rovRef - baseRef) - (rovTarget - baseTarget);
  }

  double model(const Point3& rcv, OptionalJacobian<1, 3> H_rcv = {}) const;
 
  bool equals(const DoubleDifferenceData& other, double tol) const;
};

struct GTSAM_EXPORT LeverArm {
  Point3 b{0, 0, 0};                  
  std::optional<Pose3> ecef_T_nav;    
 
  LeverArm() = default;
  explicit LeverArm(const Point3& leverArm) : b(leverArm) {}
  LeverArm(const Point3& leverArm, const Pose3& nav)
      : b(leverArm), ecef_T_nav(nav) {}

  struct PoseFrame {
    Matrix3 ecef_R_body;  
    Matrix66 H_compose;   
    bool has_nav = false;
  };

  Point3 antennaPosition(const Pose3& pose,
                         PoseFrame* frame = nullptr) const;

  Matrix16 antennaPoseJacobian(const Matrix13& H_antenna,
                               const PoseFrame& frame) const;
 
  bool equals(const LeverArm& other, double tol) const;
};
 
}  // namespace gnss
}  // namespace gtsam