.. _program_listing_file_states_state.h:

Program Listing for File state.h
================================

|exhale_lsh| :ref:`Return to documentation for file <file_states_state.h>` (``states/state.h``)

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

.. code-block:: cpp

   #pragma once
   #include <fmt/format.h>
   #include <spdlog/spdlog.h>
   
   #include "lupnt/conversions/frame_converter.h"
   #include "lupnt/core/definitions.h"
   #include "lupnt/core/error.h"
   
   namespace lupnt {
   
     using StateType = std::string_view;
   
     // State *********************************************************************
   
     class State : public VecX {
     private:
       std::string name_ = "State";
       StateType type_ = UNDEFINED;
       std::vector<std::string> names_;
       std::vector<std::string> units_;
       Frame frame_;
   
       void SetNamesAndUnits(int n);
   
     public:
       State();
   
       State(int n);
   
       State(int rows, int cols);
   
       State(const VecX& vec, const std::vector<std::string>& names,
             const std::vector<std::string>& units);
   
       template <typename T, int N> State(const State& state, const Vector<T, N>& vec);
   
       State(const State& other);
   
       template <typename Derived, typename = std::enable_if_t<(Derived::ColsAtCompileTime == 1
                                                                || Derived::RowsAtCompileTime == 1)>>
       State(const Eigen::DenseBase<Derived>& x) {
         if (x.cols() == 1) {
           VecX::operator=(x);
           SetNamesAndUnits(x.rows());
         } else if (x.rows() == 1) {
           VecX::operator=(x.transpose());
           SetNamesAndUnits(x.cols());
         } else {
           static_assert(Derived::ColsAtCompileTime == 1 || Derived::RowsAtCompileTime == 1,
                         "State can only be constructed from vectors");
         }
       }
   
       void SetFrame(Frame frame);
   
       void SetType(StateType type);
   
       void SetName(const std::string& name);
   
       void SetNames(const std::vector<std::string>& names);
   
       void SetUnits(const std::vector<std::string>& units);
   
       Frame GetFrame() const;
   
       StateType GetType() const;
   
       std::string GetName() const;
   
       std::vector<std::string> GetNames() const;
   
       std::vector<std::string> GetUnits() const;
   
       State& operator=(const State& other);
   
       template <typename Derived, typename = std::enable_if_t<(Derived::ColsAtCompileTime == 1
                                                                || Derived::RowsAtCompileTime == 1)>>
       State& operator=(const Eigen::DenseBase<Derived>& x) {
         if (x.cols() == 1) {
           VecX::operator=(x);
         } else if (x.rows() == 1) {
           VecX::operator=(x.transpose());
         } else {
           static_assert(Derived::ColsAtCompileTime == 1 || Derived::RowsAtCompileTime == 1,
                         "State can only be assigned from vectors");
         }
         return *this;
       }
   
       static constexpr StateType TYPE = UNDEFINED;
   
       std::string ToString() const;
     };
   
     template <typename T, int N> State::State(const State& state, const Vector<T, N>& vec)
         : VecX(vec) {
       type_ = state.type_;
       names_ = state.names_;
       units_ = state.units_;
       frame_ = state.frame_;
     }
   
     // Cart6 *********************************************************************
     // - $r_x$ [m] (position x-component)
     // - $r_y$ [m] (position y-component)
     // - $r_z$ [m] (position z-component)
     // - $v_x$ [m/s] (velocity x-component)
     // - $v_y$ [m/s] (velocity y-component)
     // - $v_z$ [m/s] (velocity z-component)
     class Cart6 : public State {
     private:
       void Init() {
         SetType(Cart6::TYPE);
         SetNames({"r_x", "r_y", "r_z", "v_x", "v_y", "v_z"});
         SetUnits({"m", "m", "m", "m/s", "m/s", "m/s"});
       }
   
     public:
       static constexpr StateType TYPE = "Cart6";
   
       Cart6(const State& x) : State(x) {
         LUPNT_CHECK(x.size() == 6, "State size must be 6", "Cart6");
         LUPNT_CHECK(x.GetType() == TYPE, "State type must be Cart6", "Cart6");
         Init();
       }
   
       Cart6(const Vec3& r, const Vec3& v, const Frame frame = Frame::MOON_CI) : State() {
         Init();
         SetFrame(frame);
         resize(6);
         head(3) = r;
         tail(3) = v;
       }
   
       Cart6(const Vec6& x = Vec6::Zero(), const Frame frame = Frame::MOON_CI) : State(x) {
         Init();
         SetFrame(frame);
       }
   
       Eigen::Ref<Vec3> r() { return head(3); }
   
       Eigen::Ref<Vec3> v() { return tail(3); }
   
       Vec3 r() const { return head(3); }
   
       Vec3 v() const { return tail(3); }
     };
   
     // AttitudeState ***************************************************************
     // - $q_0$ [-] (scalar part)
     // - $q_1$ [-] (x-component)
     // - $q_2$ [-] (y-component)
     // - $q_3$ [-] (z-component)
     // - $w_x$ [rad/s] (angular velocity x-component)
     // - $w_y$ [rad/s] (angular velocity y-component)
     // - $w_z$ [rad/s] (angular velocity z-component)
     class Attitude : public State {
     private:
       void Init() {
         resize(7);
         head(4) = Vec4(1.0, 0.0, 0.0, 0.0);
         tail(3) = Vec3(0.0, 0.0, 0.0);
         SetType("AttitudeState");
         SetNames({"q0", "q1", "q2", "q3", "wx", "wy", "wz"});
         SetUnits({"-", "-", "-", "-", "rad/s", "rad/s", "rad/s"});
       }
   
     public:
       static constexpr StateType TYPE = "AttitudeState";
   
       Attitude() : State() { Init(); }
   
       Attitude(int rows, int cols) : State(rows, cols) { Init(); }
   
       Attitude(const State& x) : Attitude() {
         LUPNT_CHECK(x.size() == 7, "State size must be 7", "AttitudeState");
         LUPNT_CHECK(x.GetType() == "AttitudeState", "State type must be AttitudeState",
                     "AttitudeState");
         head(7) = x.head(7);
         SetFrame(x.GetFrame());
       }
   
       Attitude(const Vec7& qw) : Attitude() { head(7) = qw; }
   
       Attitude(const Vec4& q, const Vec3& w, Frame f) : Attitude() {
         head(4) = q;
         tail(3) = w;
         SetFrame(f);
       }
   
       Eigen::Ref<Vec4> q() { return head(4); }
   
       Eigen::Ref<Vec3> w() { return tail(3); }
   
       Vec4 q() const { return head(4); }
   
       Vec3 w() const { return tail(3); }
     };
   
     // RelCart6 *******************************************************************
     // - $\delta r_x$ [m] (position x-component)
     // - $\delta r_y$ [m] (position y-component)
     // - $\delta r_z$ [m] (position z-component)
     // - $\delta v_x$ [m/s] (velocity x-component)
     // - $\delta v_y$ [m/s] (velocity y-component)
     // - $\delta v_z$ [m/s] (velocity z-component)
     class RelCart6 : public State {
     private:
       void Init() {
         resize(6);
         SetType(Cart6::TYPE);
         SetNames({"dr_x", "dr_y", "dr_z", "dv_x", "dv_y", "dv_z"});
         SetUnits({"m", "m", "m", "m/s", "m/s", "m/s"});
       }
   
     public:
       static constexpr StateType TYPE = "RelCart6";
   
       RelCart6(const State& x) : State(x) {
         LUPNT_CHECK(x.size() == 6, "State size must be 6", "RelCart6");
         LUPNT_CHECK(x.GetType() == TYPE, "State type must be RelCart6", "RelCart6");
         Init();
       }
   
       RelCart6(const Vec3& dr, const Vec3& dv, const Frame frame = Frame::MOON_CI) : State() {
         Init();
         SetFrame(frame);
         resize(6);
         head(3) = dr;
         tail(3) = dv;
       }
   
       RelCart6(const Vec6& x = Vec6::Zero(), const Frame frame = Frame::MOON_CI) : State(x) {
         Init();
         SetFrame(frame);
       }
   
       Eigen::Ref<Vec3> dr() { return head(3); }
   
       Eigen::Ref<Vec3> dv() { return tail(3); }
   
       Vec3 dr() const { return head(3); }
   
       Vec3 dv() const { return tail(3); }
     };
   
     // SurfaceState2D *************************************************************
     // - $x$ [m] (x-coordinate)
     // - $y$ [m] (y-coordinate)
     // - $\theta$ [rad] (angle)
     class SurfaceState2D : public State {
     private:
       void Init() {
         resize(3);
         SetType(SurfaceState2D::TYPE);
         SetNames({"x", "y", "theta"});
         SetUnits({"m", "m", "rad"});
       }
   
     public:
       SurfaceState2D(const State& x) : State(x) {
         LUPNT_CHECK(x.size() == 3, "State size must be 3", "Surface2DState");
         LUPNT_CHECK(x.GetType() == TYPE, "State type must be Surface2D", "Surface2DState");
         Init();
       }
   
       SurfaceState2D(const Vec3& x = Vec3::Zero(), const Frame frame = Frame::MOON_CI) : State(x) {
         Init();
         SetFrame(frame);
       }
       static constexpr StateType TYPE = "Surface2D";
     };
   
     // Cart3 **********************************************************************
     // - $r_x$ [m] (position x-component)
     // - $r_y$ [m] (position y-component)
     // - $r_z$ [m] (position z-component)
     class Cart3 : public State {
     private:
       void Init() {
         resize(3);
         SetType(Cart3::TYPE);
         SetNames({"r_x", "r_y", "r_z"});
         SetUnits({"m", "m", "m"});
       }
   
     public:
       Cart3(const State& x) : State(x) {
         LUPNT_CHECK(x.size() == 3, "State size must be 3", "Cart3");
         LUPNT_CHECK(x.GetType() == TYPE, "State type must be Cart3", "Cart3");
         Init();
       }
   
       Cart3(const Vec3& x = Vec3::Zero(), const Frame frame = Frame::MOON_CI) : State(x) {
         Init();
         SetFrame(frame);
       }
       static constexpr StateType TYPE = "Cart3";
     };
   
     // Cart2 **********************************************************************
     // - $x$ [m] (x-coordinate)
     // - $y$ [m] (y-coordinate)
     class Cart2 : public State {
     private:
       void Init() {
         resize(2);
         SetType(Cart2::TYPE);
         SetNames({"x", "y"});
         SetUnits({"m", "m"});
       }
   
     public:
       Cart2(const State& x) : State(x) {
         LUPNT_CHECK(x.size() == 2, "State size must be 2", "CartTo");
         LUPNT_CHECK(x.GetType() == TYPE, "State type must be CartTo", "CartTo");
         Init();
       }
   
       Cart2(const Vec2& x = Vec2::Zero(), const Frame frame = Frame::MOON_CI) : State(x) {
         Init();
         SetFrame(frame);
       }
       static constexpr StateType TYPE = "CartTo";
   
       Eigen::Ref<Vec2> r() { return head(2); }
   
       Vec2 r() const { return head(2); }
     };
   
     // AzElRange ******************************************************************
     // - $az$ [rad] (azimuth)
     // - $el$ [rad] (elevation)
     // - $range$ [m] (range)
     class AzElRange : public State {
     private:
       void Init() {
         resize(3);
         SetType(AzElRange::TYPE);
         SetNames({"az", "el", "range"});
         SetUnits({"rad", "rad", "m"});
       }
   
     public:
       AzElRange(const State& x) : State(x) {
         LUPNT_CHECK(x.size() == 3, "State size must be 3", "AzElRange");
         LUPNT_CHECK(x.GetType() == TYPE, "State type must be AzElRange", "AzElRange");
         Init();
       }
   
       AzElRange(const Vec3& x = Vec3::Zero(), const Frame frame = Frame::MOON_CI) : State() {
         Init();
         head(3) = x;
         SetFrame(frame);
       }
       static constexpr StateType TYPE = "AzElRange";
     };  // namespace lupnt
   
     // LatLonAlt ******************************************************************
     // - $lat$ [rad] (latitude)
     // - $lon$ [rad] (longitude)
     // - $alt$ [m] (altitude)
     class LatLonAlt : public State {
     private:
       void Init() {
         resize(3);
         SetType(LatLonAlt::TYPE);
         SetNames({"lat", "lon", "alt"});
         SetUnits({"rad", "rad", "m"});
       }
   
     public:
       LatLonAlt(const State& x) : State() {
         LUPNT_CHECK(x.size() == 3, "State size must be 3", "LatLonAlt");
         LUPNT_CHECK(x.GetType() == TYPE, "State type must be LatLonAlt", "LatLonAlt");
         Init();
         head(3) = x.head(3);
       }
   
       LatLonAlt(const Vec3& x = Vec3::Zero(), const Frame frame = Frame::MOON_CI) : State() {
         Init();
         head(3) = x;
         SetFrame(frame);
       }
       static constexpr StateType TYPE = "LatLonAlt";
     };
   
     // ClassicalOE *****************************************************************
     // Singular at $e \in {0,1}$, and $i \in {0, \pi}$
     // - $a$ [m] (semi-major axis)
     // - $e$ [-] (eccentricity)
     // - $i$ [rad] (inclination)
     // - $\Omega$ [rad] (right ascension of the ascending node)
     // - $\omega$ [rad] (argument of periapsis)
     // - $M$ [rad] (mean anomaly)
     class ClassicalOE : public State {
     private:
       void Init() {
         SetType(ClassicalOE::TYPE);
         SetNames({"a", "e", "i", "Omega", "w", "M"});
         SetUnits({"m", "-", "rad", "rad", "rad", "rad"});
       }
   
     public:
       ClassicalOE(const State& x) : State(x) {
         LUPNT_CHECK(x.size() == 6, "State size must be 6", "ClassicalOE");
         LUPNT_CHECK(x.GetType() == TYPE, "State type must be ClassicalOE", "ClassicalOE");
         Init();
       }
   
       ClassicalOE(const Vec6& x = Vec6::Zero(), const Frame frame = Frame::MOON_CI) : State(x) {
         Init();
         SetFrame(frame);
       }
       static constexpr StateType TYPE = "ClassicalOE";
   
       Real& a() { return this->coeffRef(0); }
       Real& e() { return this->coeffRef(1); }
       Real& i() { return this->coeffRef(2); }
       Real& Omega() { return this->coeffRef(3); }
       Real& w() { return this->coeffRef(4); }
       Real& M() { return this->coeffRef(5); }
   
       Real a() const { return this->coeff(0); }
       Real e() const { return this->coeff(1); }
       Real i() const { return this->coeff(2); }
       Real Omega() const { return this->coeff(3); }
       Real w() const { return this->coeff(4); }
       Real M() const { return this->coeff(5); }
     };
   
     // QuasiNonsingularOE **********************************************************
     // - $a$ [m] (semi-major axis)
     // - $u$ [-] (mean argument of latitude)
     // - $e_x$ [-] (eccentricity x-component)
     // - $e_y$ [-] (eccentricity y-component)
     // - $i$ [rad] (inclination)
     // - $\Omega$ [rad] (right ascension of the ascending node)
     // - $\omega$ [rad] (argument of periapsis)
     class QuasiNonsingularOE : public State {
     private:
       void Init() {
         SetType(QuasiNonsingularOE::TYPE);
         SetNames({"a", "u", "ex", "ey", "i", "Omega"});
         SetUnits({"m", "-", "-", "-", "rad", "rad"});
       }
   
     public:
       QuasiNonsingularOE(const State& x) : State(x) {
         LUPNT_CHECK(x.size() == 6, "State size must be 6", "QuasiNonsingularOE");
         LUPNT_CHECK(x.GetType() == TYPE, "State type must be QuasiNonsingularOE",
                     "QuasiNonsingularOE");
         Init();
       }
   
       QuasiNonsingularOE(const Vec6& x = Vec6::Zero()) : State(x) { Init(); }
   
       QuasiNonsingularOE(const Vec6& x, const Frame frame) : QuasiNonsingularOE(x) {
         SetFrame(frame);
       }
       static constexpr StateType TYPE = "QuasiNonsingularOE";
   
       Real& a() { return this->coeffRef(0); }
       Real& u() { return this->coeffRef(1); }
       Real& ex() { return this->coeffRef(2); }
       Real& ey() { return this->coeffRef(3); }
       Real& i() { return this->coeffRef(4); }
       Real& Omega() { return this->coeffRef(5); }
   
       Real a() const { return this->coeff(0); }
       Real u() const { return this->coeff(1); }
       Real ex() const { return this->coeff(2); }
       Real ey() const { return this->coeff(3); }
       Real i() const { return this->coeff(4); }
       Real Omega() const { return this->coeff(5); }
   
       StateType GetType() const { return "QuasiNonsingularOE"; }
     };
   
     // DelaunayOE ******************************************************************
     // - $l$ [rad] (mean longitude)
     // - $g$ [rad] (longitude of periapsis)
     // - $h$ [rad] (longitude of ascending node)
     // - $L$ [rad]
     // - $G$ [rad]
     // - $H$ [rad]
     class DelaunayOE : public State {
     private:
       void Init() {
         SetType(DelaunayOE::TYPE);
         SetNames({"l", "g", "h", "L", "G", "H"});
         SetUnits({"rad", "rad", "rad", "rad", "rad", "rad"});
       }
   
     public:
       DelaunayOE(const Vec6& x = Vec6::Zero()) : State(x) { Init(); }
   
       DelaunayOE(const Vec6& x, const Frame frame) : DelaunayOE(x) {
         Init();
         SetFrame(frame);
       }
       static constexpr StateType TYPE = "DelaunayOE";
   
       Real& l() { return this->coeffRef(0); }
       Real& g() { return this->coeffRef(1); }
       Real& h() { return this->coeffRef(2); }
       Real& L() { return this->coeffRef(3); }
       Real& G() { return this->coeffRef(4); }
       Real& H() { return this->coeffRef(5); }
   
       Real l() const { return this->coeff(0); }
       Real g() const { return this->coeff(1); }
       Real h() const { return this->coeff(2); }
       Real L() const { return this->coeff(3); }
       Real G() const { return this->coeff(4); }
       Real H() const { return this->coeff(5); }
     };
   
     // EquinoctialOE ***************************************************************
     // - $a$ [m] (semi-major axis)
     // - $h$ [-]
     // - $k$ [-]
     // - $p$ [-]
     // - $q$ [-]
     // - $\lambda$ [rad]
     class EquinoctialOE : public State {
     public:
       EquinoctialOE(const Vec6& x = Vec6::Zero()) : State(x) {
         SetType(TYPE);
         SetNames({"a", "h", "k", "p", "q", "lon"});
         SetUnits({"m", "-", "-", "-", "-", "rad"});
         SetFrame(Frame::MOON_CI);
       }
   
       EquinoctialOE(const Vec6& x, const Frame frame) : EquinoctialOE(x) { SetFrame(frame); }
       static constexpr StateType TYPE = "EquinoctialOE";
   
       Real& a() { return this->coeffRef(0); }
       Real& h() { return this->coeffRef(1); }
       Real& k() { return this->coeffRef(2); }
       Real& p() { return this->coeffRef(3); }
       Real& q() { return this->coeffRef(4); }
       Real& lon() { return this->coeffRef(5); }
   
       Real a() const { return this->coeff(0); }
       Real h() const { return this->coeff(1); }
       Real k() const { return this->coeff(2); }
       Real p() const { return this->coeff(3); }
       Real q() const { return this->coeff(4); }
       Real lon() const { return this->coeff(5); }
     };
   
     // SingularROE *****************************************************************
     // - $a\delta a$ [m] (semi-major axis)
     // - $a\delta M$ [m] (mean anomaly)
     // - $a\delta e$ [m] (eccentricity)
     // - $a\delta \omega$ [m] (argument of periapsis)
     // - $a\delta i$ [m] (inclination)
     // - $a\delta \Omega$ [m] (right ascension of the ascending node)
     class SingularROE : public State {
     public:
       SingularROE(const Vec6& x = Vec6::Zero()) : State(x) {
         SetType(TYPE);
         SetNames({"ada", "adM", "ade", "adw", "adi", "adOmega"});
         SetUnits({"m", "m", "m", "m", "m", "m"});
         SetFrame(Frame::MOON_CI);
       }
   
       SingularROE(const Vec6& x, const Frame frame) : SingularROE(x) { SetFrame(frame); }
       static constexpr StateType TYPE = "SingularROE";
   
       Real& ada() { return this->coeffRef(0); }
       Real& adM() { return this->coeffRef(1); }
       Real& ade() { return this->coeffRef(2); }
       Real& adw() { return this->coeffRef(3); }
       Real& adi() { return this->coeffRef(4); }
       Real& adOmega() { return this->coeffRef(5); }
   
       Real ada() const { return this->coeff(0); }
       Real adM() const { return this->coeff(1); }
       Real ade() const { return this->coeff(2); }
       Real adw() const { return this->coeff(3); }
       Real adi() const { return this->coeff(4); }
       Real adOmega() const { return this->coeff(5); }
     };
   
     // QuasiNonsingROE ************************************************************
     // - $a\delta a$ [m] (semi-major axis)
     // - $a\delta l$ [m] (mean longitude)
     // - $a\delta e_x$ [m] (eccentricity x-component)
     // - $a\delta e_y$ [m] (eccentricity y-component)
     // - $a\delta i_x$ [m] (inclination x-component)
     // - $a\delta i_y$ [m] (inclination y-component)
     class QuasiNonsingROE : public State {
     public:
       QuasiNonsingROE(const Vec6& x = Vec6::Zero()) : State(x) {
         SetType(TYPE);
         SetNames({"ada", "adl", "adex", "adey", "adix", "adiy"});
         SetUnits({"m", "m", "m", "m", "m", "m"});
         SetFrame(Frame::MOON_CI);
       }
   
       QuasiNonsingROE(const Vec6& x, const Frame frame) : QuasiNonsingROE(x) { SetFrame(frame); }
       static constexpr StateType TYPE = "QuasiNonsingROE";
   
       Real& ada() { return this->coeffRef(0); }
       Real& adl() { return this->coeffRef(1); }
       Real& adex() { return this->coeffRef(2); }
       Real& adey() { return this->coeffRef(3); }
       Real& adix() { return this->coeffRef(4); }
       Real& adiy() { return this->coeffRef(5); }
   
       Real ada() const { return this->coeff(0); }
       Real adl() const { return this->coeff(1); }
       Real adex() const { return this->coeff(2); }
       Real adey() const { return this->coeff(3); }
       Real adix() const { return this->coeff(4); }
       Real adiy() const { return this->coeff(5); }
     };
   
     // RollPitchYaw ****************************************************************
     // - $roll$ [rad] (roll)
     // - $pitch$ [rad] (pitch)
     // - $yaw$ [rad] (yaw)
     class RollPitchYaw : public State {
     public:
       RollPitchYaw(const Vec3& x = Vec3::Zero()) : State(x) {
         SetType(TYPE);
         SetNames({"roll", "pitch", "yaw"});
         SetUnits({"rad", "rad", "rad"});
       }
       static constexpr StateType TYPE = "RollPitchYaw";
   
       Real& roll() { return this->coeffRef(0); }
       Real& pitch() { return this->coeffRef(1); }
       Real& yaw() { return this->coeffRef(2); }
   
       Real roll() const { return this->coeff(0); }
       Real pitch() const { return this->coeff(1); }
       Real yaw() const { return this->coeff(2); }
     };
   
     // Quaternion ****************************************************************
     // - $q_0$ [-] (scalar part)
     // - $q_1$ [-] (x-component)
     // - $q_2$ [-] (y-component)
     // - $q_3$ [-] (z-component)
     class Quaternion : public State {
     public:
       Quaternion(const Vec4& x = Vec4::Zero()) : State(x) {
         SetType(TYPE);
         SetNames({"q_0", "q_1", "q_2", "q_3"});
         SetUnits({"-", "-", "-", "-"});
       }
       static constexpr StateType TYPE = "Quaternion";
   
       Real& q0() { return this->coeffRef(0); }
       Real& q1() { return this->coeffRef(1); }
       Real& q2() { return this->coeffRef(2); }
       Real& q3() { return this->coeffRef(3); }
     };
   
     // ImuState *******************************************************************
     // - $b\omega_x$ [rad/s] (body frame angular velocity x-component)
     // - $b\omega_y$ [rad/s] (body frame angular velocity y-component)
     // - $b\omega_z$ [rad/s] (body frame angular velocity z-component)
     // - $a_x$ [m/s^2] (body frame acceleration x-component)
     // - $a_y$ [m/s^2] (body frame acceleration y-component)
     class ImuState : public State {
     private:
       void Init() {
         resize(SIZE);
         setZero();
         SetType(TYPE);
         SetNames({"b_w_x", "b_w_y", "b_w_z", "b_a_x", "b_a_y", "b_a_z"});
         SetUnits({"rad/s", "rad/s", "rad/s", "m/s^2", "m/s^2", "m/s^2"});
       }
   
       void Check(const State& x) {
         LUPNT_CHECK(x.size() == SIZE, "State size must be 6", "ImuState");
         LUPNT_CHECK(x.GetType() == TYPE, "State type must be ImuState", "ImuState");
       }
   
     public:
       ImuState(const Vec6& x = Vec6::Zero()) : State() {
         Init();
         head(SIZE) = x;
       }
   
       ImuState(const State& x) : State() {
         Check(x);
         Init();
         head(SIZE) = x.head(SIZE);
       }
   
       ImuState& operator=(const State& x) {
         if (this != &x) {
           Check(x);
           Init();
           head(SIZE) = x.head(SIZE);
         }
         return *this;
       }
   
       Eigen::Ref<Vec3> b_w() { return head(3); }
       Eigen::Ref<Vec3> b_a() { return tail(3); }
       Vec3 b_w() const { return head(3); }
       Vec3 b_a() const { return tail(3); }
   
       static constexpr StateType TYPE = "ImuState";
       static constexpr int SIZE = 6;
     };
   
     // ImuMeasurement *************************************************************
     // - $w_x$ [rad/s] (angular velocity x-component)
     // - $w_y$ [rad/s] (angular velocity y-component)
     // - $w_z$ [rad/s] (angular velocity z-component)
     class AngularVelocity : public State {
     public:
       AngularVelocity(const Vec3& x = Vec3::Zero()) : State(x) {
         SetType(TYPE);
         SetNames({"w_x", "w_y", "w_z"});
         SetUnits({"rad/s", "rad/s", "rad/s"});
       }
       static constexpr StateType TYPE = "AngularVelocity";
   
       Real& wx() { return this->coeffRef(0); }
       Real& wy() { return this->coeffRef(1); }
       Real& wz() { return this->coeffRef(2); }
   
       Real wx() const { return this->coeff(0); }
       Real wy() const { return this->coeff(1); }
       Real wz() const { return this->coeff(2); }
     };
   
     class Acceleration : public State {
     public:
       Acceleration(const Vec3& x = Vec3::Zero()) : State(x) {
         SetType(TYPE);
         SetNames({"a_x", "a_y", "a_z"});
         SetUnits({"m/s^2", "m/s^2", "m/s^2"});
       }
       static constexpr StateType TYPE = "Acceleration";
   
       Real& ax() { return this->coeffRef(0); }
       Real& ay() { return this->coeffRef(1); }
       Real& az() { return this->coeffRef(2); }
   
       Real ax() const { return this->coeff(0); }
       Real ay() const { return this->coeff(1); }
       Real az() const { return this->coeff(2); }
     };
   
     // ClockState ******************************************************************
     class ClockState3 : public State {
     private:
       void Init() {
         resize(3);
         setZero();
         SetType("ClockState");
         SetNames({"b", "d", "dr"});
         SetUnits({"s", "s/s", "s/s^2"});
         SetFrame(Frame::MOON_CI);
       }
   
     public:
       ClockState3() : State() { Init(); }
   
       ClockState3(int rows, int cols) : State(rows, cols) { Init(); }
   
       ClockState3(const State& x) : State() {
         LUPNT_CHECK(x.size() == 3, "State must be of size 2 or 3", "ClockState");
         Init();
         head(3) = x.head(3);
       }
   
       ClockState3& operator=(const State& x) {
         if (this != &x) {
           LUPNT_CHECK(x.size() == 3, "State must be of size 2 or 3", "ClockState");
           head(3) = x.head(3);
         }
         return *this;
       }
   
       Real& b() { return this->coeffRef(0); }
       Real& d() { return this->coeffRef(1); }
       Real& dr() { return this->coeffRef(2); }
   
       Real b() const { return this->coeff(0); }
       Real d() const { return this->coeff(1); }
       Real dr() const { return this->coeff(2); }
     };
   
     // ClockState ******************************************************************
     class ClockState2 : public State {
     private:
       void Init() {
         resize(2);
         setZero();
         SetType("ClockState");
         SetNames({"b", "d"});
         SetUnits({"s", "s/s"});
         SetFrame(Frame::MOON_CI);
       }
   
     public:
       ClockState2() : State() { Init(); }
   
       ClockState2(int rows, int cols) : State(rows, cols) { Init(); }
   
       ClockState2(const State& x) : State() {
         LUPNT_CHECK(x.size() == 2, "State must be of size 2 or 3", "ClockState");
         Init();
         head(2) = x.head(2);
       }
   
       ClockState2& operator=(const State& x) {
         if (this != &x) {
           LUPNT_CHECK(x.size() == 2, "State must be of size 2 or 3", "ClockState");
           head(2) = x.head(2);
         }
         return *this;
       }
   
       Real& b() { return this->coeffRef(0); }
       Real& d() { return this->coeffRef(1); }
   
       Real b() const { return this->coeff(0); }
       Real d() const { return this->coeff(1); }
     };
   
     // JointOrbitClockState ********************************************************
     // - Cartesian orbit state followed by a 2- or 3-state clock model.
   
     class JointOrbitClockState : public State {
     private:
       static constexpr int ORBIT_STATE_SIZE = 6;
       int clock_state_size_ = 3;
   
       void Init(int clock_size, const std::vector<std::string>& clock_units) {
         LUPNT_CHECK(clock_size == 2 || clock_size == 3, "Clock state size must be 2 or 3",
                     "JointOrbitClockState");
         clock_state_size_ = clock_size;
         resize(ORBIT_STATE_SIZE + clock_state_size_);
         SetType(JointOrbitClockState::TYPE);
         std::vector<std::string> names = {"r_x", "r_y", "r_z", "v_x", "v_y", "v_z", "b", "d"};
         if (clock_state_size_ == 3) names.push_back("dr");
         SetNames(names);
   
         std::vector<std::string> units = {"m", "m", "m", "m/s", "m/s", "m/s"};
         units.insert(units.end(), clock_units.begin(), clock_units.end());
         SetUnits(units);
       }
   
       std::vector<std::string> GetClockUnitsFromState(const State& clock_state) const {
         std::vector<std::string> units = clock_state.GetUnits();
         if (units.size() == static_cast<size_t>(clock_state.size())) return units;
         return clock_state.size() == 2 ? std::vector<std::string>{"s", "s/s"}
                                        : std::vector<std::string>{"s", "s/s", "s/s^2"};
       }
   
     public:
       static constexpr StateType TYPE = "JointOrbitClock";
   
       JointOrbitClockState() : State() {
         setZero(9);
         Init(3, {"s", "s/s", "s/s^2"});
         SetFrame(Frame::MOON_CI);
       }
   
       explicit JointOrbitClockState(int clock_size) : State() {
         setZero(ORBIT_STATE_SIZE + clock_size);
         Init(clock_size, clock_size == 2 ? std::vector<std::string>{"s", "s/s"}
                                          : std::vector<std::string>{"s", "s/s", "s/s^2"});
         SetFrame(Frame::MOON_CI);
       }
   
       JointOrbitClockState(const State& x) : State(x) {
         LUPNT_CHECK(x.size() == 8 || x.size() == 9, "State size must be 8 or 9",
                     "JointOrbitClockState");
         clock_state_size_ = x.size() - ORBIT_STATE_SIZE;
         SetType(JointOrbitClockState::TYPE);
         if (x.GetNames().size() != static_cast<size_t>(x.size())
             || x.GetUnits().size() != static_cast<size_t>(x.size())) {
           Init(clock_state_size_, clock_state_size_ == 2
                                       ? std::vector<std::string>{"s", "s/s"}
                                       : std::vector<std::string>{"s", "s/s", "s/s^2"});
           head(x.size()) = x.head(x.size());
           SetFrame(x.GetFrame());
         }
       }
   
       JointOrbitClockState(const State& orbit_state, const State& clock_state) : State() {
         LUPNT_CHECK(orbit_state.size() == ORBIT_STATE_SIZE, "Orbit state size must be 6",
                     "JointOrbitClockState");
         LUPNT_CHECK(clock_state.size() == 2 || clock_state.size() == 3,
                     "Clock state size must be 2 or 3", "JointOrbitClockState");
         Init(clock_state.size(), GetClockUnitsFromState(clock_state));
         head(ORBIT_STATE_SIZE) = orbit_state.head(ORBIT_STATE_SIZE);
         tail(clock_state.size()) = clock_state.head(clock_state.size());
         SetFrame(orbit_state.GetFrame());
       }
   
       JointOrbitClockState& operator=(const State& x) {
         LUPNT_CHECK(x.size() == 8 || x.size() == 9, "State size must be 8 or 9",
                     "JointOrbitClockState");
         if (this != &x) {
           clock_state_size_ = x.size() - ORBIT_STATE_SIZE;
           State::operator=(x);
           SetType(JointOrbitClockState::TYPE);
         }
         return *this;
       }
   
       int GetOrbitStateSize() const { return ORBIT_STATE_SIZE; }
   
       int GetClockStateSize() const { return clock_state_size_; }
   
       Cart6 GetOrbitState() const {
         Cart6 orbit(head<ORBIT_STATE_SIZE>(), GetFrame());
         return orbit;
       }
   
       State GetClockState() const {
         State clock(clock_state_size_);
         clock.head(clock_state_size_) = tail(clock_state_size_);
         clock.SetType("ClockState");
         clock.SetNames(clock_state_size_ == 2 ? std::vector<std::string>{"b", "d"}
                                               : std::vector<std::string>{"b", "d", "dr"});
         std::vector<std::string> joint_units = GetUnits();
         std::vector<std::string> units(joint_units.begin() + ORBIT_STATE_SIZE, joint_units.end());
         clock.SetUnits(units);
         clock.SetFrame(GetFrame());
         return clock;
       }
   
       void SetOrbitState(const State& orbit_state) {
         LUPNT_CHECK(orbit_state.size() == ORBIT_STATE_SIZE, "Orbit state size must be 6",
                     "JointOrbitClockState");
         head(ORBIT_STATE_SIZE) = orbit_state.head(ORBIT_STATE_SIZE);
         SetFrame(orbit_state.GetFrame());
       }
   
       void SetClockState(const State& clock_state) {
         LUPNT_CHECK(clock_state.size() == clock_state_size_, "Clock state size mismatch",
                     "JointOrbitClockState");
         tail(clock_state_size_) = clock_state.head(clock_state_size_);
         std::vector<std::string> units = GetUnits();
         std::vector<std::string> clock_units = GetClockUnitsFromState(clock_state);
         std::copy(clock_units.begin(), clock_units.end(), units.begin() + ORBIT_STATE_SIZE);
         SetUnits(units);
       }
   
       Eigen::Ref<Vec3> r() { return head(3); }
       Eigen::Ref<Vec3> v() { return segment(3, 3); }
       Real& b() { return this->coeffRef(ORBIT_STATE_SIZE); }
       Real& d() { return this->coeffRef(ORBIT_STATE_SIZE + 1); }
   
       Real& dr() {
         LUPNT_CHECK(clock_state_size_ == 3, "Clock state does not contain drift rate",
                     "JointOrbitClockState");
         return this->coeffRef(ORBIT_STATE_SIZE + 2);
       }
   
       Vec3 r() const { return head(3); }
       Vec3 v() const { return segment(3, 3); }
       Real b() const { return this->coeff(ORBIT_STATE_SIZE); }
       Real d() const { return this->coeff(ORBIT_STATE_SIZE + 1); }
   
       Real dr() const {
         LUPNT_CHECK(clock_state_size_ == 3, "Clock state does not contain drift rate",
                     "JointOrbitClockState");
         return this->coeff(ORBIT_STATE_SIZE + 2);
       }
     };
   
   }  // namespace lupnt
   
   template <> struct fmt::formatter<lupnt::State> : fmt::formatter<std::string> {
     template <typename FormatContext>
     auto format(const lupnt::State& state, FormatContext& ctx) const {
       std::ostringstream oss;
       oss << state.GetType() << "(" << state.transpose().format(lupnt::FMT_COMPACT) << ", "
           << enum_name(state.GetFrame()) << ")";
       return fmt::formatter<std::string>::format(oss.str(), ctx);
     }
   };
   
   template <> struct fmt::formatter<lupnt::Cart6> : fmt::formatter<std::string> {
     template <typename FormatContext>
     auto format(const lupnt::Cart6& state, FormatContext& ctx) const {
       std::ostringstream oss;
       oss << state.GetType() << "(" << state.transpose().format(lupnt::FMT_COMPACT) << ", "
           << enum_name(state.GetFrame()) << ")";
       return formatter<std::string>::format(oss.str(), ctx);
     }
   };