Skip to content

Commit 7562bb4

Browse files
astomodynamicsastomodynamics
authored
Add ROE model (#93)
1 parent 60002f6 commit 7562bb4

File tree

6 files changed

+532
-106
lines changed

6 files changed

+532
-106
lines changed

‎CMakeLists.txt

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -243,7 +243,9 @@ set(dynamics_model_srcs
243243
src/dynamics_model/spacecraft_nonlinear.cpp
244244
src/dynamics_model/dreyfus_rocket.cpp
245245
src/dynamics_model/spacecraft_landing2d.cpp
246+
src/dynamics_model/spacecraft_roe.cpp
246247
src/dynamics_model/lti_system.cpp
248+
247249
)
248250

249251
add_library(${PROJECT_NAME}

‎include/cddp-cpp/cddp.hpp

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -49,6 +49,7 @@
4949
#include "dynamics_model/spacecraft_landing2d.hpp"
5050
#include "dynamics_model/lti_system.hpp"
5151
#include "dynamics_model/dreyfus_rocket.hpp"
52+
#include "dynamics_model/spacecraft_roe.hpp"
5253

5354
#include "matplot/matplot.h"
5455

‎include/cddp-cpp/dynamics_model/spacecraft_roe.hpp

Lines changed: 138 additions & 106 deletions
Original file line numberDiff line numberDiff line change
@@ -23,114 +23,146 @@
2323
namespace cddp
2424
{
2525

26-
/**
27-
* @brief Spacecraft Relative Orbital Elements (ROE) Dynamics
28-
*
29-
* State vector (6 dimensions):
30-
* x = [ da, dlambda, dex, dey, dix, diy ]
31-
* where:
32-
* - da : Relative semi-major axis
33-
* - dlambda : Relative mean longitude (or some variant of relative angle)
34-
* - dex : Relative x-component of eccentricity vector
35-
* - dey : Relative y-component of eccentricity vector
36-
* - dix : Relative x-component of inclination vector
37-
* - diy : Relative y-component of inclination vector
38-
*
39-
* Control vector (3 dimensions):
40-
* u = [ ur, ut, un ]
41-
* where:
42-
* - ur : radial acceleration [m/s^2]
43-
* - ut : tangential acceleration [m/s^2]
44-
* - un : normal acceleration [m/s^2]
45-
*/
46-
class SpacecraftROE : public DynamicalSystem
47-
{
48-
public:
49-
/**
50-
* @brief Constructor
51-
* @param timestep Integration timestep [s]
52-
* @param integration_type Integration method (e.g. "rk4")
53-
* @param mass Spacecraft mass [kg]
54-
* @param mu Gravitational parameter [m^3/s^2]
55-
* @param n_ref Mean motion of the reference orbit [rad/s]
56-
*/
57-
SpacecraftROE(
58-
double timestep,
59-
const std::string& integration_type,
60-
double mass,
61-
double mu,
62-
double n_ref);
63-
64-
// State indices
65-
static constexpr int STATE_DA = 0; ///< Relative semi-major axis
66-
static constexpr int STATE_DLAMBDA = 1; ///< Relative mean longitude
67-
static constexpr int STATE_DEX = 2; ///< Relative eccentricity x-component
68-
static constexpr int STATE_DEY = 3; ///< Relative eccentricity y-component
69-
static constexpr int STATE_DIX = 4; ///< Relative inclination x-component
70-
static constexpr int STATE_DIY = 5; ///< Relative inclination y-component
71-
static constexpr int STATE_DIM = 6; ///< State dimension
72-
73-
// Control indices
74-
static constexpr int CONTROL_UR = 0; ///< Radial acceleration
75-
static constexpr int CONTROL_UT = 1; ///< Tangential acceleration
76-
static constexpr int CONTROL_UN = 2; ///< Normal acceleration
77-
static constexpr int CONTROL_DIM = 3; ///< Control dimension
78-
79-
/**
80-
* @brief Compute continuous-time dynamics in ROE coordinates
81-
* @param state Current ROE state vector
82-
* @param control Current control vector [ur, ut, un]
83-
* @return Derivative of the ROE state vector
84-
*/
85-
Eigen::VectorXd getContinuousDynamics(
86-
const Eigen::VectorXd& state,
87-
const Eigen::VectorXd& control) const override;
88-
89-
/**
90-
* @brief Compute state Jacobian matrix via numerical finite difference
91-
* @param state Current ROE state vector
92-
* @param control Current control vector
93-
* @return State Jacobian matrix (d f / d state)
94-
*/
95-
Eigen::MatrixXd getStateJacobian(
96-
const Eigen::VectorXd& state,
97-
const Eigen::VectorXd& control) const override;
98-
99-
/**
100-
* @brief Compute control Jacobian matrix via numerical finite difference
101-
* @param state Current ROE state vector
102-
* @param control Current control vector
103-
* @return Control Jacobian matrix (d f / d control)
104-
*/
105-
Eigen::MatrixXd getControlJacobian(
106-
const Eigen::VectorXd& state,
107-
const Eigen::VectorXd& control) const override;
108-
109-
/**
110-
* @brief Compute state Hessian matrix
111-
* @param state Current state vector
112-
* @param control Current control vector
113-
* @return State Hessian matrix
114-
*/
115-
Eigen::MatrixXd getStateHessian(
116-
const Eigen::VectorXd& state,
117-
const Eigen::VectorXd& control) const override;
118-
11926
/**
120-
* @brief Compute control Hessian matrix
121-
* @param state Current state vector
122-
* @param control Current control vector
123-
* @return Control Hessian matrix
27+
* @brief Spacecraft Relative Orbital Elements (ROE) Dynamics
28+
*
29+
* State vector (6 dimensions):
30+
* x = [ da, dlambda, dex, dey, dix, diy ]
31+
* where:
32+
* - da : Relative semi-major axis
33+
* - dlambda : Relative mean longitude (or some variant of relative angle)
34+
* - dex : Relative x-component of eccentricity vector
35+
* - dey : Relative y-component of eccentricity vector
36+
* - dix : Relative x-component of inclination vector
37+
* - diy : Relative y-component of inclination vector
38+
*
39+
* Control vector (3 dimensions):
40+
* u = [ ur, ut, un ]
41+
* where:
42+
* - ur : radial acceleration [m/s^2]
43+
* - ut : tangential acceleration [m/s^2]
44+
* - un : normal acceleration [m/s^2]
12445
*/
125-
Eigen::MatrixXd getControlHessian(
126-
const Eigen::VectorXd& state,
127-
const Eigen::VectorXd& control) const override;
128-
129-
private:
130-
double mass_; ///< Spacecraft mass [kg]
131-
double mu_; ///< Gravitational parameter [m^3/s^2]
132-
double n_ref_; ///< Mean motion of reference orbit [rad/s]
133-
};
46+
class SpacecraftROE : public DynamicalSystem
47+
{
48+
public:
49+
/**
50+
* @brief Constructor
51+
* @param timestep Integration timestep [s]
52+
* @param integration_type Integration method (e.g. "rk4")
53+
* @param a Semi-major axis of the reference orbit [m]
54+
* @param u0 Initial mean argument of latitude [rad]
55+
*/
56+
SpacecraftROE(
57+
double timestep,
58+
const std::string &integration_type,
59+
double a,
60+
double u0 = 0.0);
61+
62+
// State indices
63+
static constexpr int STATE_DA = 0; ///< Relative semi-major axis
64+
static constexpr int STATE_DLAMBDA = 1; ///< Relative mean longitude
65+
static constexpr int STATE_DEX = 2; ///< Relative eccentricity x-component
66+
static constexpr int STATE_DEY = 3; ///< Relative eccentricity y-component
67+
static constexpr int STATE_DIX = 4; ///< Relative inclination x-component
68+
static constexpr int STATE_DIY = 5; ///< Relative inclination y-component
69+
static constexpr int STATE_DIM = 6; ///< State dimension
70+
71+
// Control indices
72+
static constexpr int CONTROL_UR = 0; ///< Radial acceleration
73+
static constexpr int CONTROL_UT = 1; ///< Tangential acceleration
74+
static constexpr int CONTROL_UN = 2; ///< Normal acceleration
75+
static constexpr int CONTROL_DIM = 3; ///< Control dimension
76+
77+
/**
78+
* @brief Compute continuous-time dynamics in ROE coordinates
79+
* @param state Current ROE state vector
80+
* @param control Current control vector [ur, ut, un]
81+
* @return Derivative of the ROE state vector
82+
*/
83+
Eigen::VectorXd getContinuousDynamics(
84+
const Eigen::VectorXd &state,
85+
const Eigen::VectorXd &control) const override;
86+
87+
/**
88+
* @brief Compute state Jacobian matrix via numerical finite difference
89+
* @param state Current ROE state vector
90+
* @param control Current control vector
91+
* @return State Jacobian matrix (d f / d state)
92+
*/
93+
Eigen::MatrixXd getStateJacobian(
94+
const Eigen::VectorXd &state,
95+
const Eigen::VectorXd &control) const override;
96+
97+
/**
98+
* @brief Compute control Jacobian matrix via numerical finite difference
99+
* @param state Current ROE state vector
100+
* @param control Current control vector
101+
* @return Control Jacobian matrix (d f / d control)
102+
*/
103+
Eigen::MatrixXd getControlJacobian(
104+
const Eigen::VectorXd &state,
105+
const Eigen::VectorXd &control) const override;
106+
107+
/**
108+
* @brief Compute state Hessian matrix
109+
* @param state Current state vector
110+
* @param control Current control vector
111+
* @return State Hessian matrix
112+
*/
113+
Eigen::MatrixXd getStateHessian(
114+
const Eigen::VectorXd &state,
115+
const Eigen::VectorXd &control) const override;
116+
117+
/**
118+
* @brief Compute control Hessian matrix
119+
* @param state Current state vector
120+
* @param control Current control vector
121+
* @return Control Hessian matrix
122+
*/
123+
Eigen::MatrixXd getControlHessian(
124+
const Eigen::VectorXd &state,
125+
const Eigen::VectorXd &control) const override;
126+
127+
/**
128+
* @brief Transform the QNS-ROE state to the local Hill/Clohessy-Wiltshire frame.
129+
*
130+
* The returned vector is [x, y, z, xdot, ydot, zdot], representing the relative
131+
* position and velocity in the Hill frame.
132+
*
133+
* @param roe A 6D QNS-ROE state vector
134+
* @param t Time since epoch [s]
135+
* @return A 6D vector [x, y, z, xdot, ydot, zdot] in the HCW frame
136+
*/
137+
Eigen::VectorXd transformROEToHCW(const Eigen::VectorXd &roe, double t) const;
138+
139+
/**
140+
* @brief Transform a HCW state to the QNS-ROE state.
141+
*
142+
* Expects a 6D input [x, y, z, xdot, ydot, zdot], representing the relative
143+
* position and velocity in the Hill/Clohessy‐Wiltshire frame.
144+
*
145+
* @param hcw A 6D Hill/CWH state vector
146+
* @param t Time since epoch [s] (used for the orbit phase)
147+
* @return A 6D QNS-ROE vector [da, dlambda, dex, dey, dix, diy]
148+
*/
149+
Eigen::VectorXd transformHCWToROE(const Eigen::VectorXd &hcw, double t) const;
150+
151+
double getSemiMajorAxis() const { return a_; }
152+
double getMeanMotion() const { return n_ref_; }
153+
double getInitialMeanArgumentOfLatitude() const { return u0_; }
154+
double getGravitationalParameter() const { return mu_; }
155+
void setGravitationalParameter(double mu) { mu_ = mu; }
156+
void setMeanMotion(double n) { n_ref_ = n; }
157+
void setSemiMajorAxis(double a) { a_ = a; }
158+
void setInitialMeanArgumentOfLatitude(double u0) { u0_ = u0; }
159+
160+
private:
161+
double a_; ///< Semi-major axis of the reference orbit [m]
162+
double n_ref_; ///< Mean motion of the reference orbit [rad/s]
163+
double u0_; ///< Initial mean argu- ment of latitude [rad]
164+
double mu_ = 3.9860044e14; ///< Gravitational parameter [m^3/s^2]
165+
};
134166

135167
} // namespace cddp
136168

0 commit comments

Comments
 (0)