.. _program_listing_file_dynamics_clock_dynamics.cc:

Program Listing for File clock_dynamics.cc
==========================================

|exhale_lsh| :ref:`Return to documentation for file <file_dynamics_clock_dynamics.cc>` (``dynamics/clock_dynamics.cc``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   #include "lupnt/dynamics/clock_dynamics.h"
   
   #include <tuple>
   
   #include "lupnt/core/asset_factory.h"
   #include "lupnt/core/constants.h"
   #include "lupnt/core/definitions.h"
   #include "lupnt/devices/clock.h"
   #include "lupnt/numerics/math_utils.h"
   
   namespace lupnt {
   
     // ClockDynamics
     // A. C. Long, ‘Goddard Enhanced Onboard Navigation System (GEONS) Mathematical
     // Specifications’, 2024.
     std::tuple<double, double, double> ClockDynamics::GetClockValues(ClockModel clk_model) {
       double q1 = 0.0;  // [s]
       double q2 = 0.0;  // [1/s]
       double q3 = 0.0;  // [1/s^2]
   
       switch (clk_model) {
         case ClockModel::OCXO:
           // Preliminary Navigation System Design for the First LCRNS
           // Satellite Providing Lunar PNT Services
           q1 = pow(6.2299445014e-13, 2);
           q2 = pow(2.0129544799e-14, 2);
           q3 = pow(7.0118586804e-28, 2);
           break;
         case ClockModel::USO:
           // MMS USO
           q1 = 1.2e-22;
           q2 = 1.58e-26;
           q3 = 5.9e-75;
           break;
         case ClockModel::CSAC:
           q1 = 5.8e-22;
           q2 = 1.5e-29;
           q3 = 1e-45;
           break;
         case ClockModel::MINI_RAFS:
           q1 = pow(1.0e-11, 2);
           q2 = pow(1.1e-15, 2);
           q3 = pow(7.0118586804e-28, 2);
           break;
         case ClockModel::RAFS:
           // https://www.nasa.gov/wp-content/uploads/2025/08/gps-based-autonomous-navigation-study-for-the-lunar-gateway-1.pdf?emrc=68cc48e722112
           q1 = 3.70e-24;
           q2 = 1.87e-33;
           q3 = 7.56e-59;
           break;
         case ClockModel::DSAC:
           q1 = 4.23e-26;
           q2 = 6.19e-38;
           q3 = 2.24e-61;
           break;
         default: LUPNT_CHECK(false, "Unknown clock model", "ClockDynamics");
       }
       return std::make_tuple(q1, q2, q3);
     }
   
     ClockDynamics::ClockDynamics() { model_ = ClockModel::UNDEFINED; }
   
     ClockDynamics::ClockDynamics(Config& config) {
       model_ = ClockModel::UNDEFINED;
       if (config["model"]) {
         model_ = enum_cast<ClockModel>(config["model"].as<std::string>()).value();
       }
       if (config["clock_bias_unit"]) {
         bias_unit_ = enum_cast<ClockBiasUnit>(config["clock_bias_unit"].as<std::string>()).value();
       } else if (config["bias_unit"]) {
         bias_unit_ = enum_cast<ClockBiasUnit>(config["bias_unit"].as<std::string>()).value();
       }
     }
   
     double ClockDynamics::SecondsToBiasUnitScale(ClockBiasUnit unit) {
       switch (unit) {
         case ClockBiasUnit::SECONDS: return 1.0;
         case ClockBiasUnit::METERS: return C;
         case ClockBiasUnit::KILOMETERS: return C * KM_M;
         default: LUPNT_CHECK(false, "Unknown clock bias unit", "ClockDynamics");
       }
     }
   
     Real ClockDynamics::SecondsToBiasUnits(Real value_s, ClockBiasUnit unit) {
       return value_s * SecondsToBiasUnitScale(unit);
     }
   
     Real ClockDynamics::BiasUnitsToSeconds(Real value, ClockBiasUnit unit) {
       return value / SecondsToBiasUnitScale(unit);
     }
   
     std::vector<std::string> ClockDynamics::GetStateUnits(int state_size, ClockBiasUnit unit) {
       std::vector<std::string> units;
       switch (unit) {
         case ClockBiasUnit::SECONDS: units = {"s", "s/s", "s/s^2"}; break;
         case ClockBiasUnit::METERS: units = {"m", "m/s", "m/s^2"}; break;
         case ClockBiasUnit::KILOMETERS: units = {"km", "km/s", "km/s^2"}; break;
         default: LUPNT_CHECK(false, "Unknown clock bias unit", "ClockDynamics");
       }
   
       LUPNT_CHECK(state_size == 2 || state_size == 3, "Clock state size must be 2 or 3",
                   "ClockDynamics");
       units.resize(state_size);
       return units;
     }
   
     Real ClockDynamics::RelativisticRateCorrection(const Vec3& r_centered, const Vec3& v_centered,
                                                    Real GM, Real c, Real reference_rate_offset) {
       Real r = r_centered.norm();
       LUPNT_CHECK(r > 0.0, "Relativistic clock correction requires a nonzero radius",
                   "ClockDynamics");
   
       Real v2 = v_centered.squaredNorm();
       return reference_rate_offset - (GM / r + 0.5 * v2) / (c * c);
     }
   
     Mat2 ClockDynamics::TwoStateNoise(ClockModel clk_model, Real dt) {
       Real q1, q2, q3;
       std::tie(q1, q2, q3) = GetClockValues(clk_model);
   
       Real dt2 = dt * dt;
       Real dt3 = dt2 * dt;
   
       Mat2 Q;
       Q(0, 0) = q1 * dt + q2 * dt3 / 3.0;
       Q(0, 1) = q2 * dt2 / 2.0;
       Q(1, 0) = Q(0, 1);
       Q(1, 1) = q2 * dt;
       return Q;
     };
   
     Mat2 ClockDynamics::TwoStateNoise(ClockModel clk_model, Real dt, ClockBiasUnit unit) {
       Real scale = SecondsToBiasUnitScale(unit);
       return TwoStateNoise(clk_model, dt) * scale * scale;
     }
   
     Mat3 ClockDynamics::ThreeStateNoise(ClockModel clk_model, Real dt) {
       Real q1, q2, q3;
       std::tie(q1, q2, q3) = GetClockValues(clk_model);
   
       Real dt2 = dt * dt;
       Real dt3 = dt2 * dt;
       Real dt4 = dt3 * dt;
       Real dt5 = dt4 * dt;
   
       Mat3 Q;
       Q(0, 0) = q1 * dt + q2 * dt3 / 3 + q3 * dt5 / 20.0;
       Q(0, 1) = q2 * dt2 / 2.0 + q3 * dt4 / 8.0;
       Q(0, 2) = q3 * dt3 / 6.0;
       Q(1, 0) = Q(0, 1);
       Q(1, 1) = q2 * dt + q3 * dt3 / 3.0;
       Q(1, 2) = q3 * dt2 / 2.0;
       Q(2, 0) = Q(0, 2);
       Q(2, 1) = Q(1, 2);
       Q(2, 2) = q3 * dt;
       return Q;
     };
   
     Mat3 ClockDynamics::ThreeStateNoise(ClockModel clk_model, Real dt, ClockBiasUnit unit) {
       Real scale = SecondsToBiasUnitScale(unit);
       return ThreeStateNoise(clk_model, dt) * scale * scale;
     }
   
     VecX ClockDynamics::GetProcessNoise(Real dt, int state_size) {
       MatX Q;
       VecX noise(state_size);
       if (state_size == 2) {
         Q = ClockDynamics::TwoStateNoise(model_, dt, bias_unit_);
         noise = SampleMvNormal(Vec2d::Zero(), Q, 1, rng_.get()).transpose();
       } else if (state_size == 3) {
         Q = ClockDynamics::ThreeStateNoise(model_, dt, bias_unit_);
         noise = SampleMvNormal(Vec3d::Zero(), Q, 1, rng_.get()).transpose();
       } else {
         LUPNT_CHECK(false, fmt::format("Invalid clock state size {}", state_size), "ClockDynamics");
       }
       return noise;
     }
   
     Mat2 ClockDynamics::TwoStatePhi(Real dt) {
       Mat2 Phi_clk{{1, dt}, {0, 1}};
       return Phi_clk;
     }
   
     Mat3 ClockDynamics::ThreeStatePhi(Real dt) {
       Mat3 Phi_clk{{1, dt, dt * dt / 2}, {0, 1, dt}, {0, 0, 1}};
       return Phi_clk;
     }
   
     State ClockDynamics::Propagate(const State& x0, Real t0, Real tf, const State* u) {
       return PropagateImpl(x0, t0, tf, u, nullptr, nullptr);
     }
   
     State ClockDynamics::Propagate(const State& x0, Real t0, Real tf, const State* u, MatXd* stm) {
       return PropagateImpl(x0, t0, tf, u, nullptr, stm);
     }
   
     State ClockDynamics::PropagateWithRelativity(const State& x0, Real t0, Real tf,
                                                  const ClockRelativityContext& relativity,
                                                  const State* u, MatXd* stm) {
       return PropagateImpl(x0, t0, tf, u, &relativity, stm);
     }
   
     State ClockDynamics::PropagateImpl(const State& x0, Real t0, Real tf, const State* u,
                                        const ClockRelativityContext* relativity, MatXd* stm) {
       LUPNT_CHECK(u == nullptr, "ClockDynamics does not support control inputs", "ClockDynamics");
   
       MatXd Phi_clk, Q_clk;
       VecX noise(x0.size());
       if (x0.size() == 2) {
         Phi_clk = TwoStatePhi(tf - t0).cast<double>();
         if (model_ != ClockModel::UNDEFINED && add_noise_) {
           Q_clk = TwoStateNoise(model_, tf - t0, bias_unit_).cast<double>();
           noise = SampleMvNormal(Vec2d::Zero(), Q_clk, 1, rng_.get()).transpose();
         }
       } else if (x0.size() == 3) {
         Phi_clk = ThreeStatePhi(tf - t0).cast<double>();
         if (model_ != ClockModel::UNDEFINED && add_noise_) {
           Q_clk = ThreeStateNoise(model_, tf - t0, bias_unit_).cast<double>();
           noise = SampleMvNormal(Vec3d::Zero(), Q_clk, 1, rng_.get()).transpose();
         }
       } else {
         LUPNT_CHECK(false, fmt::format("Invalid clock state size {}", x0.size()), "ClockDynamics");
       }
   
       VecX xf = Phi_clk * x0;
       if (relativity != nullptr) {
         Real rate0 = RelativisticRateCorrection(relativity->rv0_centered.head<3>(),
                                                 relativity->rv0_centered.tail<3>(), relativity->GM,
                                                 relativity->c, relativity->reference_rate_offset);
         Real ratef = RelativisticRateCorrection(relativity->rvf_centered.head<3>(),
                                                 relativity->rvf_centered.tail<3>(), relativity->GM,
                                                 relativity->c, relativity->reference_rate_offset);
         Real dt = tf - t0;
         Real bias_rel_s = 0.5 * (rate0 + ratef) * dt;
         xf(0) += SecondsToBiasUnits(bias_rel_s, bias_unit_);
       }
       if (model_ != ClockModel::UNDEFINED && add_noise_) xf += noise;
       State x_state(x0, xf);
       x_state.SetUnits(GetStateUnits(x0.size(), bias_unit_));
       if (stm != nullptr) *stm = Phi_clk.cast<double>();
       return x_state;
     }
   
     REGISTER_FACTORY_CLASS(ClockDynamics, ClockDynamics)
   
     // Define the GetRegistry function for this specialization (must come before explicit
     // instantiation)
     template <> std::unordered_map<std::string, AssetFactory<ClockDynamics, Config&>::Creator>&
     AssetFactory<ClockDynamics, Config&>::GetRegistry() {
       return Registry();
     }
   
     // Explicit template instantiation to ensure single registry across library boundaries
     template class AssetFactory<ClockDynamics, Config&>;
   
   }  // namespace lupnt