Program Listing for File gnss_measurement.h

↰ Return to documentation for file (measurements/gnss_measurement.h)

#pragma once

#include <functional>
#include <optional>
#include <vector>

#include "lupnt/agents/gnss_constellation.h"
#include "lupnt/dynamics/clock_dynamics.h"
#include "lupnt/environment/plasma/gcpm/iri_interface.h"
#include "lupnt/environment/plasma/tec/raytrace.h"
#include "lupnt/filters/filter.h"
#include "lupnt/measurements/measurement.h"
#include "lupnt/numerics/cheby_fit.h"

namespace lupnt {

  enum class GnssObservable { PSEUDORANGE, DOPPLER, CARRIER_PHASE };

  struct GnssMeasurementStateIndices {
    int position = 0;
    int velocity = 3;
    int clock_bias = 6;
    int clock_drift = 7;
    int carrier_integer = -1;
  };

  struct GnssMeasurementOptions {
    std::vector<GnssObservable> observables
        = {GnssObservable::PSEUDORANGE, GnssObservable::DOPPLER, GnssObservable::CARRIER_PHASE};
    GnssMeasurementStateIndices indices;
    ClockBiasUnit clock_bias_unit = ClockBiasUnit::SECONDS;

    Time receive_time_scale = Time::TAI;

    Time ephemeris_time_scale = Time::TAI;

    Frame frame = Frame::ECI;

    bool solve_light_time = true;
    bool recompute_transmit_time_from_ephemeris = false;
    int light_time_max_iterations = 10;
    Real light_time_tolerance_s = 1.0e-11;
    bool apply_transmitter_relativity = true;
    bool apply_shapiro_delay = true;
    Real shapiro_mu = GM_SUN;
    Vec3 shapiro_body_position = Vec3::Zero();
    bool apply_visibility = true;
    bool apply_cn0_threshold = true;
    Real cn0_threshold_dbhz = 15.0;
    bool apply_ionosphere_plasma_delay = false;
    Real default_ionosphere_plasma_delay_m = 0.0;
  };

  struct GnssChannel {
    GnssConst gnss_const = GnssConst::GPS;
    int prn = 0;
    GnssFreq frequency = GnssFreq::L1;
    Real receive_time = 0.0;
    Time receive_time_scale = Time::TAI;
    Real transmit_time = 0.0;
    Time transmit_time_scale = Time::TAI;
    Time ephemeris_time_scale = Time::TAI;
    Frame frame = Frame::ECI;

    Vec6 tx_state = Vec6::Zero();
    VecXd ephemeris_times;
    MatXd ephemeris_tx_states;
    ChebyshevFitModel ephemeris_chebyshev;
    Real tx_clock_bias_s = 0.0;
    Real tx_clock_drift = 0.0;
    Real relativistic_correction_s = 0.0;
    Real group_delay_s = 0.0;
    Real shapiro_delay_m = 0.0;
    Real ionosphere_plasma_delay_m = 0.0;
    Real phase_bias_cycles = 0.0;
    Real integer_ambiguity_cycles = 0.0;

    Real cn0_dbhz = NAN;
    Real sigma_pseudorange_m = NAN;
    Real sigma_doppler_hz = NAN;
    Real sigma_carrier_phase_cycles = NAN;

    Real Wavelength() const;

    bool HasEphemeris() const;

    Vec6 GetTransmitState(Real t) const;

    Real EffectiveTransmitClockBiasSeconds() const;

    Real EffectiveTransmitClockDrift() const;
  };

  struct GnssMeasurementValue {
    Real pseudorange_m = NAN;
    Real doppler_hz = NAN;
    Real carrier_phase_cycles = NAN;
    Real carrier_integer_cycles = 0.0;

    VecXd AsVector(const std::vector<GnssObservable>& observables) const;
  };

  struct GNSSMeasurementsEpoch {
    Real receive_time = 0.0;
    Time receive_time_scale = Time::TAI;
    Real time = 0.0;  // Backward-compatible alias of `receive_time`.

    std::vector<GnssChannel> channels;
    VecXd values;
    MatXd jacobian;
    MatXd covariance;
  };

  enum class GnssIonospherePlasmaRayTraceDelayMode { TEC_ONLY, TEC_PLUS_HIGHER_ORDER };

  struct GnssIonospherePlasmaRayTraceOptions {
    pecsim::RayTraceConfig config;

    Frame raytrace_frame = Frame::ECEF;

    Time raytrace_epoch_scale = Time::UTC;

    bool use_channel_frequency = true;

    pecsim::IRIModel iri_model = pecsim::IRIModel::NONE;

    GnssIonospherePlasmaRayTraceDelayMode delay_mode
        = GnssIonospherePlasmaRayTraceDelayMode::TEC_ONLY;
    bool debug_propagation = false;
    bool debug_correction = false;
  };

  class GNSSMeasurements;

  class GnssMeasurement : public Measurement {
  public:
    GnssMeasurement() = default;

    explicit GnssMeasurement(const GnssChannel& channel);

    void SetChannel(const GnssChannel& channel) { channel_ = channel; }

    const GnssChannel& GetChannel() const { return channel_; }

    GnssMeasurementValue ComputeValue(const State& user_state,
                                      const GnssMeasurementOptions& options = {}) const;

    VecXd Compute(const State& user_state, MatXd* H = nullptr,
                  const GnssMeasurementOptions& options = {}) const;

    FilterMeasurementFunction CreateFunction(const GnssMeasurementOptions& options = {}) const;

    static std::vector<GNSSMeasurementsEpoch> Precompute(const GNSSMeasurements& measurements,
                                                         const std::vector<Real>& receive_times,
                                                         const std::vector<State>& user_states,
                                                         bool compute_jacobians = false);

  private:
    GnssChannel channel_;

    static Real ClockBiasToMeters(Real bias, ClockBiasUnit unit);

    static Real ClockDriftToMetersPerSecond(Real drift, ClockBiasUnit unit);

    static Real ClockBiasMetersDerivative(ClockBiasUnit unit);

    static Real ClockDriftMetersPerSecondDerivative(ClockBiasUnit unit);
  };

  class GNSSMeasurements {
  public:
    using VectorProvider = std::function<Vec3(Real)>;
    using CustomIonospherePlasmaDelayModel
        = std::function<Real(Real, const Vec3&, const Vec3&, GnssFreq)>;
    using BatchCustomIonospherePlasmaDelayModel = std::function<std::vector<std::vector<Real>>(
        const std::vector<Real>&, const std::vector<State>&,
        const std::vector<std::vector<GnssChannel>>&)>;

    GNSSMeasurements() = default;

    explicit GNSSMeasurements(Ptr<GnssConstellation> constellation);

    void SetConstellation(Ptr<GnssConstellation> constellation) { constellation_ = constellation; }

    Ptr<GnssConstellation> GetConstellation() const { return constellation_; }

    void SetFrequency(GnssFreq frequency) { frequency_ = frequency; }

    GnssFreq GetFrequency() const { return frequency_; }

    void SetOptions(const GnssMeasurementOptions& options) { options_ = options; }

    const GnssMeasurementOptions& GetOptions() const { return options_; }

    void SetSunPositionProvider(VectorProvider provider) { sun_position_provider_ = provider; }

    void SetBoresightTargetProvider(VectorProvider provider) {
      boresight_target_provider_ = provider;
    }

    void SetOccludingBodies(const std::vector<GnssOccludingBody>& bodies) {
      occluding_bodies_ = bodies;
    }

    void SetReceiverParams(const GnssReceiverParams& params) { rx_params_ = params; }

    void SetReceiverAntenna(const Antenna& antenna) { rx_antenna_ = antenna; }

    void SetCN0Threshold(Real cn0_threshold_dbhz) {
      options_.cn0_threshold_dbhz = cn0_threshold_dbhz;
    }

    void SetCustomIonospherePlasmaDelayModel(CustomIonospherePlasmaDelayModel model) {
      custom_ionosphere_plasma_delay_model_ = model;
    }

    void SetBatchCustomIonospherePlasmaDelayModel(BatchCustomIonospherePlasmaDelayModel model) {
      batch_custom_ionosphere_plasma_delay_model_ = model;
    }

    [[deprecated("Use SetCustomIonospherePlasmaDelayModel")]]
    void SetIonospherePlasmaDelayProvider(CustomIonospherePlasmaDelayModel provider) {
      SetCustomIonospherePlasmaDelayModel(provider);
    }

    [[deprecated("Use SetBatchCustomIonospherePlasmaDelayModel")]]
    void SetBatchIonospherePlasmaDelayProvider(BatchCustomIonospherePlasmaDelayModel provider) {
      SetBatchCustomIonospherePlasmaDelayModel(provider);
    }

    void SetIonospherePlasmaRayTraceOptions(const GnssIonospherePlasmaRayTraceOptions& options) {
      ionosphere_plasma_raytrace_options_ = options;
    }

    void ClearIonospherePlasmaRayTraceOptions() { ionosphere_plasma_raytrace_options_.reset(); }

    static bool ComputeVisibility(const Vec3& r1, const Vec3& r2, Real R_body,
                                  const Vec3& r_body = Vec3::Zero());

    std::vector<GnssChannel> BuildChannels(Real receive_time, const State& user_state) const;

    GNSSMeasurementsEpoch Compute(Real receive_time, const State& user_state,
                                  MatXd* H = nullptr) const;

    std::vector<GNSSMeasurementsEpoch> Precompute(const std::vector<Real>& receive_times,
                                                  const std::vector<State>& user_states,
                                                  bool compute_jacobians = false);

    const std::vector<GNSSMeasurementsEpoch>& GetPrecomputed() const { return precomputed_; }

    FilterMeasurementFunction CreateFunction(Real t) const;

  private:
    Ptr<GnssConstellation> constellation_;
    GnssFreq frequency_ = GnssFreq::L1;
    GnssMeasurementOptions options_;
    std::vector<GnssOccludingBody> occluding_bodies_;
    Antenna rx_antenna_;
    GnssReceiverParams rx_params_;
    VectorProvider sun_position_provider_;
    VectorProvider boresight_target_provider_;
    CustomIonospherePlasmaDelayModel custom_ionosphere_plasma_delay_model_;
    BatchCustomIonospherePlasmaDelayModel batch_custom_ionosphere_plasma_delay_model_;
    std::optional<GnssIonospherePlasmaRayTraceOptions> ionosphere_plasma_raytrace_options_;
    std::vector<GNSSMeasurementsEpoch> precomputed_;

    Vec3 SunPosition(Real t) const;

    Vec3 BoresightTarget(Real t) const;

    GnssChannel BuildChannelForPrn(int prn, Real receive_time, const State& user_state,
                                   bool compute_ionosphere_plasma_delay = true) const;

    std::vector<GnssChannel> BuildChannels(Real receive_time, const State& user_state,
                                           bool compute_ionosphere_plasma_delay) const;

    GNSSMeasurementsEpoch ComputeFromChannels(Real receive_time, const State& user_state,
                                              std::vector<GnssChannel> channels,
                                              MatXd* H = nullptr) const;

    Real ComputeShapiroDelay(Real receive_time, const Vec3& r_rx_eci, const Vec3& r_tx_eci) const;

    Real ComputeIonospherePlasmaDelay(Real receive_time, const Vec3& r_rx_eci, const Vec3& r_tx_eci,
                                      GnssFreq freq) const;

    Real ComputeIonospherePlasmaRayTraceDelay(Real receive_time, const Vec3& r_rx, const Vec3& r_tx,
                                              GnssFreq freq) const;

    Real ComputeCN0(const GnssChannel& channel, const Vec3& r_rx_eci, Real receive_time) const;

    Real ComputeSigmaRange(Real cn0_dbhz, GnssFreq freq) const;

    Real ComputeSigmaRangeRate(Real cn0_dbhz, GnssFreq freq) const;

    Real ComputeSigmaCarrierPhase(Real cn0_dbhz, GnssFreq freq) const;
  };

}  // namespace lupnt