Program Listing for File state.h

↰ Return to documentation for file (states/state.h)

#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);
  }
};