Program Listing for File clock_dynamics.h

↰ Return to documentation for file (dynamics/clock_dynamics.h)

#pragma once

#include <tuple>

#include "lupnt/conversions/frame_converter.h"
#include "lupnt/core/constants.h"
#include "lupnt/core/definitions.h"
#include "lupnt/dynamics/dynamics.h"
#include "lupnt/states/state.h"

namespace lupnt {

  enum class ClockModel;
  enum class ClockBiasUnit { SECONDS, METERS, KILOMETERS };

  struct ClockRelativityContext {
    Cart6 rv0_centered;
    Cart6 rvf_centered;
    Real GM = 0.0;
    Real c = C;
    Real reference_rate_offset = 0.0;
  };

  class ClockDynamics : public Dynamics {
  protected:
    int seed_ = 0;
    Ptr<std::mt19937> rng_ = nullptr;
    ClockModel model_;
    bool add_noise_ = true;
    ClockBiasUnit bias_unit_ = ClockBiasUnit::SECONDS;

  private:
    State PropagateImpl(const State& x0, Real t0, Real tf, const State* u,
                        const ClockRelativityContext* relativity, MatXd* stm);

  public:
    ClockDynamics();

    ClockDynamics(Config& config);

    static std::tuple<double, double, double> GetClockValues(ClockModel clk_model);

    static double SecondsToBiasUnitScale(ClockBiasUnit unit);

    static Real SecondsToBiasUnits(Real value_s, ClockBiasUnit unit);

    static Real BiasUnitsToSeconds(Real value, ClockBiasUnit unit);

    static std::vector<std::string> GetStateUnits(int state_size, ClockBiasUnit unit);

    static Real RelativisticRateCorrection(const Vec3& r_centered, const Vec3& v_centered, Real GM,
                                           Real c = C, Real reference_rate_offset = 0.0);

    void SetSeed(int seed) {
      seed_ = seed;
      rng_ = MakePtr<std::mt19937>(seed_);
    }
    int GetSeed() const { return seed_; }

    void SetModel(ClockModel model) { model_ = model; }
    ClockModel GetModel() const { return model_; }
    void SetAddNoise(bool add_noise) { add_noise_ = add_noise; }
    bool GetAddNoise() const { return add_noise_; }
    void SetClockBiasUnit(ClockBiasUnit unit) { bias_unit_ = unit; }
    ClockBiasUnit GetClockBiasUnit() const { return bias_unit_; }

    VecX GetProcessNoise(Real dt, int state_size);

    static Mat2 TwoStatePhi(Real dt);

    static Mat3 ThreeStatePhi(Real dt);

    static Mat2 TwoStateNoise(ClockModel clk_model, Real dt);

    static Mat2 TwoStateNoise(ClockModel clk_model, Real dt, ClockBiasUnit unit);

    static Mat3 ThreeStateNoise(ClockModel clk_model, Real dt);

    static Mat3 ThreeStateNoise(ClockModel clk_model, Real dt, ClockBiasUnit unit);

    using Dynamics::Propagate;

    State Propagate(const State& x0, Real t0, Real tf, const State* u, MatXd* stm) override;

    State Propagate(const State& x0, Real t0, Real tf, const State* u = nullptr) override;

    State PropagateWithRelativity(const State& x0, Real t0, Real tf,
                                  const ClockRelativityContext& relativity,
                                  const State* u = nullptr, MatXd* stm = nullptr);

    StateType GetStateType() const override { return ClockState3::TYPE; }
  };
}  // namespace lupnt