From 77606a89c1133a756f5d5dafa892536a5b57361f Mon Sep 17 00:00:00 2001 From: Vishwa Shah Date: Tue, 7 Nov 2023 17:39:48 -0800 Subject: [PATCH] feat: Q-Law guidance law (#266) * wip: first cut at guidance law * wip: feat: add tests * feat: fix remaining tests * feat: address Remy's feedback: * refactor: move guidance law around * feat: fix tests * style: format * wip: first cut at guidance law * feat: QLaw guidance law * wip: Use finite difference solver * feat: add working unit tests * feat: add bindings and python test * feat: add tests for COE * feat: add end-to-end test * feat: add more tests * feat: fix some small bugs * feat: address remaining feedback * feat: add python tests/bindings for COE and fix thruster test py * feat: fix tests * feat: fix test and printing * feat: fix test --- .../GuidanceLaw.cpp | 2 + .../GuidanceLaw/QLaw.cpp | 196 ++++++++++ .../Trajectory/Orbit/Models/Kepler/COE.cpp | 172 +++++++-- .../python/test/guidance_law/test_qlaw.py | 140 +++++++ .../orbit/models/kepler/test_coe.py | 20 + .../GuidanceLaw/ConstantThrust.hpp | 4 +- .../Astrodynamics/GuidanceLaw/QLaw.hpp | 173 +++++++++ .../Trajectory/Orbit/Models/Kepler/COE.hpp | 59 ++- .../Astrodynamics/Dynamics/Thruster.cpp | 8 +- .../Astrodynamics/GuidanceLaw/QLaw.cpp | 348 ++++++++++++++++++ .../Trajectory/Orbit/Models/Kepler/COE.cpp | 80 ++++ .../Astrodynamics/GuidanceLaw/QLaw.test.cpp | 155 ++++++++ .../Orbit/Models/Kepler/COE.test.cpp | 74 ++++ .../Trajectory/Propagator.test.cpp | 202 +++++++++- .../Trajectory/Sequence.test.cpp | 2 +- 15 files changed, 1590 insertions(+), 45 deletions(-) create mode 100644 bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/GuidanceLaw/QLaw.cpp create mode 100644 bindings/python/test/guidance_law/test_qlaw.py create mode 100644 include/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/QLaw.hpp create mode 100644 src/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/QLaw.cpp create mode 100644 test/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/QLaw.test.cpp diff --git a/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/GuidanceLaw.cpp b/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/GuidanceLaw.cpp index 74eca4803..b18e2c0ca 100644 --- a/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/GuidanceLaw.cpp +++ b/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/GuidanceLaw.cpp @@ -3,6 +3,7 @@ #include #include +#include using namespace pybind11; @@ -123,4 +124,5 @@ void OpenSpaceToolkitAstrodynamicsPy_GuidanceLaw(pybind11::module& aModule) // Add objects to "guidance_law" submodule OpenSpaceToolkitAstrodynamicsPy_GuidanceLaw_ConstantThrust(guidance_law); + OpenSpaceToolkitAstrodynamicsPy_GuidanceLaw_QLaw(guidance_law); } diff --git a/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/GuidanceLaw/QLaw.cpp b/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/GuidanceLaw/QLaw.cpp new file mode 100644 index 000000000..3f8a95842 --- /dev/null +++ b/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/GuidanceLaw/QLaw.cpp @@ -0,0 +1,196 @@ +/// Apache License 2.0 + +#include + +using namespace pybind11; + +using ostk::core::types::String; +using ostk::core::types::Real; +using ostk::core::types::Shared; +using ostk::core::types::Size; +using ostk::core::ctnr::Map; + +using ostk::math::obj::Vector3d; + +using ostk::physics::time::Instant; +using ostk::physics::coord::Frame; +using ostk::physics::units::Derived; + +using ostk::astro::GuidanceLaw; +using ostk::astro::guidancelaw::QLaw; +using ostk::astro::trajectory::orbit::models::kepler::COE; +using ostk::astro::solvers::FiniteDifferenceSolver; + +void OpenSpaceToolkitAstrodynamicsPy_GuidanceLaw_QLaw(pybind11::module& aModule) +{ + class_( + aModule, + "QLawParameters", + R"doc( + Q-law parameters. + + Group: + guidance-law + )doc" + ) + .def_readonly( + "m", + &QLaw::Parameters::m, + R"doc( + Scaling parameter for SMA delta. + + Type: + int + )doc" + ) + .def_readonly( + "n", + &QLaw::Parameters::n, + R"doc( + Scaling parameter for SMA delta. + + Type: + int + )doc" + ) + .def_readonly( + "r", + &QLaw::Parameters::r, + R"doc( + Scaling parameter for SMA delta. + + Type: + int + )doc" + ) + .def_readonly( + "b", + &QLaw::Parameters::b, + R"doc( + Scaling parameter for SMA delta. + + Type: + float + )doc" + ) + + .def( + init&, const Size&, const Size&, const Size&, const Real&>(), + R"doc( + Constructor. + + Args: + element_weights (dict): Key-value pair of COE elements and the weights for the targeter. + m (int): Scaling parameter for SMA delta. + n (int): Scaling parameter for SMA delta. + r (int): Scaling parameter for SMA delta. + b (float): Scaling parameter for SMA delta. + + )doc", + arg("element_weights"), + arg("m"), + arg("n"), + arg("r"), + arg("b") + ) + + .def( + "get_control_weights", + &QLaw::Parameters::getControlWeights, + R"doc( + Get the control weights. + + Returns: + np.array: The control weights. + )doc" + ) + + ; + + class_>( + aModule, + "QLaw", + R"doc( + This class implements the Q-law guidance law. + + Ref: https://dataverse.jpl.nasa.gov/api/access/datafile/10307?gbrecs=true + Ref: https://www.researchgate.net/publication/370849580_Analytic_Calculation_and_Application_of_the_Q-Law_Guidance_Algorithm_Partial_Derivatives + Ref for derivations: https://dataverse.jpl.nasa.gov/api/access/datafile/13727?gbrecs=true + + The Q-law is a Lyapunov feedback control law developed by Petropoulos, + based on analytic expressions for maximum rates of change of the orbit elements and + the desired changes in the elements. Q, the proximity quotient, serves as a candidate Lyapunov + function. As the spacecraft approaches the target orbit, Q decreases monotonically (becoming zero at the target orbit). + + Group: + guidance-law + )doc" + ) + + .def("__str__", &(shiftToString)) + .def("__repr__", &(shiftToString)) + + .def( + init(), + R"doc( + Constructor. + + Args: + coe (COE): The target orbit described by Classical Orbital Elements. + gravitational_parameter (float): The gravitational parameter of the central body. + parameters (QLaw.Parameters): A set of parameters for the QLaw. + finite_difference_solver (FiniteDifferenceSolver): The finite difference solver. + + )doc", + arg("target_coe"), + arg("gravitational_parameter"), + arg("parameters"), + arg("finite_difference_solver") + ) + + .def( + "get_parameters", + &QLaw::getParameters, + R"doc( + Get the parameters. + + Returns: + QLaw.Parameters: The parameters. + )doc" + ) + .def( + "get_target_coe", + &QLaw::getTargetCOE, + R"doc( + Get the target COE. + + Returns: + COE: The target COE. + )doc" + ) + + .def( + "calculate_thrust_acceleration_at", + &GuidanceLaw::calculateThrustAccelerationAt, + R"doc( + Calculate the thrust acceleration at the provided coordinates and instant. + + Args: + instant (Instant): Instant of computation. + position_coordinates (np.array): Position coordinates. + velocity_coordinates (np.array): Velocity coordinates. + thrust_acceleration (float): Thrust acceleration magnitude. + output_frame (Frame): The frame the acceleration is expressed in. + + Returns: + np.array: The acceleration. + )doc", + arg("instant"), + arg("position_coordinates"), + arg("velocity_coordinates"), + arg("thrust_acceleration"), + arg("output_frame") + ) + + ; +} diff --git a/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/Trajectory/Orbit/Models/Kepler/COE.cpp b/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/Trajectory/Orbit/Models/Kepler/COE.cpp index 9423b77a6..0801b5bcf 100644 --- a/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/Trajectory/Orbit/Models/Kepler/COE.cpp +++ b/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/Trajectory/Orbit/Models/Kepler/COE.cpp @@ -112,10 +112,10 @@ inline void OpenSpaceToolkitAstrodynamicsPy_Trajectory_Orbit_Models_Kepler_COE(p "is_defined", &COE::isDefined, R"doc( - Check if the COE is defined. + Check if the COE is defined. - Returns: - bool: True if the COE is defined, False otherwise. + Returns: + bool: True if the COE is defined, False otherwise. )doc" ) @@ -123,10 +123,10 @@ inline void OpenSpaceToolkitAstrodynamicsPy_Trajectory_Orbit_Models_Kepler_COE(p "get_semi_major_axis", &COE::getSemiMajorAxis, R"doc( - Get the semi-major axis of the COE. + Get the semi-major axis of the COE. - Returns: - Length: The semi-major axis of the COE. + Returns: + Length: The semi-major axis of the COE. )doc" ) @@ -134,10 +134,10 @@ inline void OpenSpaceToolkitAstrodynamicsPy_Trajectory_Orbit_Models_Kepler_COE(p "get_eccentricity", &COE::getEccentricity, R"doc( - Get the eccentricity of the COE. + Get the eccentricity of the COE. - Returns: - float: The eccentricity of the COE. + Returns: + float: The eccentricity of the COE. )doc" ) @@ -145,10 +145,10 @@ inline void OpenSpaceToolkitAstrodynamicsPy_Trajectory_Orbit_Models_Kepler_COE(p "get_inclination", &COE::getInclination, R"doc( - Get the inclination of the COE. + Get the inclination of the COE. - Returns: - Angle: The inclination of the COE. + Returns: + Angle: The inclination of the COE. )doc" ) @@ -156,10 +156,10 @@ inline void OpenSpaceToolkitAstrodynamicsPy_Trajectory_Orbit_Models_Kepler_COE(p "get_raan", &COE::getRaan, R"doc( - Get the right ascension of the ascending node of the COE. + Get the right ascension of the ascending node of the COE. - Returns: - Angle: The right ascension of the ascending node of the COE. + Returns: + Angle: The right ascension of the ascending node of the COE. )doc" ) @@ -167,10 +167,10 @@ inline void OpenSpaceToolkitAstrodynamicsPy_Trajectory_Orbit_Models_Kepler_COE(p "get_aop", &COE::getAop, R"doc( - Get the argument of periapsis of the COE. + Get the argument of periapsis of the COE. - Returns: - Angle: The argument of periapsis of the COE. + Returns: + Angle: The argument of periapsis of the COE. )doc" ) @@ -178,10 +178,10 @@ inline void OpenSpaceToolkitAstrodynamicsPy_Trajectory_Orbit_Models_Kepler_COE(p "get_true_anomaly", &COE::getTrueAnomaly, R"doc( - Get the true anomaly of the COE. + Get the true anomaly of the COE. - Returns: - Angle: The true anomaly of the COE. + Returns: + Angle: The true anomaly of the COE. )doc" ) @@ -189,10 +189,10 @@ inline void OpenSpaceToolkitAstrodynamicsPy_Trajectory_Orbit_Models_Kepler_COE(p "get_mean_anomaly", &COE::getMeanAnomaly, R"doc( - Get the mean anomaly of the COE. + Get the mean anomaly of the COE. - Returns: - Angle: The mean anomaly of the COE. + Returns: + Angle: The mean anomaly of the COE. )doc" ) @@ -200,10 +200,10 @@ inline void OpenSpaceToolkitAstrodynamicsPy_Trajectory_Orbit_Models_Kepler_COE(p "get_eccentric_anomaly", &COE::getEccentricAnomaly, R"doc( - Get the eccentric anomaly of the COE. + Get the eccentric anomaly of the COE. - Returns: - Angle: The eccentric anomaly of the COE. + Returns: + Angle: The eccentric anomaly of the COE. )doc" ) @@ -211,10 +211,10 @@ inline void OpenSpaceToolkitAstrodynamicsPy_Trajectory_Orbit_Models_Kepler_COE(p "get_periapsis_radius", &COE::getPeriapsisRadius, R"doc( - Get the periapsis radius of the COE. + Get the periapsis radius of the COE. - Returns: - Length: The periapsis radius of the COE. + Returns: + Length: The periapsis radius of the COE. )doc" ) @@ -222,10 +222,46 @@ inline void OpenSpaceToolkitAstrodynamicsPy_Trajectory_Orbit_Models_Kepler_COE(p "get_apoapsis_radius", &COE::getApoapsisRadius, R"doc( - Get the apoapsis radius of the COE. + Get the apoapsis radius of the COE. + + Returns: + Length: The apoapsis radius of the COE. + )doc" + ) + + .def( + "get_semi_latus_rectum", + &COE::getSemiLatusRectum, + R"doc( + Get the semi-latus rectum of the COE. + + Returns: + Length: The semilatus rectum of the COE. + )doc" + ) + + .def( + "get_radial_distance", + &COE::getRadialDistance, + R"doc( + Get the radial distance of the COE. Returns: - Length: The apoapsis radius of the COE. + Length: The radial distance of the COE. + )doc" + ) + + .def( + "get_angular_momentum", + &COE::getAngularMomentum, + R"doc( + Get the angular momentum of the COE. + + Args: + gravitational_parameter (Derived): The gravitational parameter of the central body. + + Returns: + Derived: The angular momentum of the COE. )doc" ) @@ -425,6 +461,78 @@ inline void OpenSpaceToolkitAstrodynamicsPy_Trajectory_Orbit_Models_Kepler_COE(p arg("tolerance") ) + .def_static( + "compute_semi_latus_rectum", + &COE::ComputeSemiLatusRectum, + R"doc( + Compute the semi-latus rectum from the semi-major axis and the eccentricity. + + Args: + semi_major_axis (float): The semi-major axis. In meters. + eccentricity (float): The eccentricity. + + Returns: + Length: The semi-latus rectum. + )doc", + arg("semi_major_axis"), + arg("eccentricity") + ) + + .def_static( + "compute_angular_momentum", + overload_cast(&COE::ComputeAngularMomentum), + R"doc( + Compute the angular momentum from the semi-major axis and the eccentricity. + + Args: + semi_major_axis (float): The semi-major axis. In meters. + eccentricity (float): The eccentricity. + gravitational_parameter (Derived): The gravitational parameter of the central body. + + Returns: + Derived: The angular momentum. + )doc", + arg("semi_major_axis"), + arg("eccentricity"), + arg("gravitational_parameter") + ) + + .def_static( + "compute_angular_momentum", + overload_cast(&COE::ComputeAngularMomentum), + R"doc( + Compute the angular momentum from the semi-latus rectum. + + Args: + semi_latus_rectum (float): The semi-latus rectum. In meters. + gravitational_parameter (Derived): The gravitational parameter of the central body. + + Returns: + Derived: The angular momentum. + )doc", + arg("semi_latus_rectum"), + arg("gravitational_parameter") + ) + + .def_static( + "compute_radial_distance", + &COE::ComputeRadialDistance, + R"doc( + Compute the radial distance from the semi-latus rectum and the eccentricity. + + Args: + semi_latus_rectum (float): The semi-latus rectum. In meters. + eccentricity (float): The eccentricity. + true_anomaly (float): The true anomly. In degrees. + + Returns: + Length: The radial distance. + )doc", + arg("semi_latus_rectum"), + arg("eccentricity"), + arg("true_anomaly") + ) + .def_static( "string_from_element", &COE::StringFromElement, diff --git a/bindings/python/test/guidance_law/test_qlaw.py b/bindings/python/test/guidance_law/test_qlaw.py new file mode 100644 index 000000000..8fe29f8fe --- /dev/null +++ b/bindings/python/test/guidance_law/test_qlaw.py @@ -0,0 +1,140 @@ +# Apache License 2.0 + +import pytest + +import numpy as np + +from ostk.physics.time import Instant +from ostk.physics.coordinate import Frame +from ostk.physics.units import Derived +from ostk.physics.environment.gravitational import Earth as EarthGravitationalModel +from ostk.physics.time import Instant +from ostk.physics.coordinate import Frame +from ostk.physics.units import Length +from ostk.physics.units import Angle +from ostk.physics.units import Derived + + +from ostk.astrodynamics.solvers import FiniteDifferenceSolver +from ostk.astrodynamics.trajectory.orbit.models.kepler import COE +from ostk.astrodynamics import GuidanceLaw +from ostk.astrodynamics.guidance_law import QLaw +from ostk.astrodynamics.guidance_law import QLawParameters + + +@pytest.fixture +def target_COE() -> COE: + return COE( + Length.meters(42000.0e3), + 0.01, + Angle.degrees(0.05), + Angle.degrees(0.0), + Angle.degrees(0.0), + Angle.degrees(0.0), + ) + + +@pytest.fixture +def gravitational_parameter() -> Derived: + return Derived( + 3.986004418e14, EarthGravitationalModel.EGM2008.gravitational_parameter.get_unit() + ) + + +@pytest.fixture +def parameters() -> QLawParameters: + return QLawParameters( + element_weights={COE.Element.SemiMajorAxis: 1.0, COE.Element.Eccentricity: 1.0}, + m=3, + n=4, + r=2, + b=0.01, + ) + + +@pytest.fixture +def finite_difference_solver() -> FiniteDifferenceSolver: + return FiniteDifferenceSolver.default() + + +@pytest.fixture +def q_law( + target_COE: COE, + gravitational_parameter: Derived, + parameters: QLawParameters, + finite_difference_solver: FiniteDifferenceSolver, +) -> QLaw: + return QLaw( + target_coe=target_COE, + gravitational_parameter=gravitational_parameter, + parameters=parameters, + finite_difference_solver=finite_difference_solver, + ) + + +@pytest.fixture +def thrust_acceleration() -> float: + return 1.0 / 300.0 + + +@pytest.fixture +def frame() -> Frame: + return Frame.GCRF() + + +@pytest.fixture +def position_coordinates() -> list[float]: + return [6930000.0, 0.0, 0.0] + + +@pytest.fixture +def velocity_coordinates() -> list[float]: + return [0.0, 7621.89248591193, 6.65135764404186] + + +@pytest.fixture +def instant() -> Instant: + return Instant.J2000() + + +class TestQLawParameters: + def test_constructors(self, parameters: QLawParameters): + assert parameters is not None + assert isinstance(parameters, QLawParameters) + + def test_getters(self, parameters: QLawParameters): + assert parameters.get_control_weights() is not None + assert parameters.m is not None + assert parameters.n is not None + assert parameters.r is not None + assert parameters.b is not None + + +class TestQLaw: + def test_constructors(self, q_law: QLaw): + assert q_law is not None + assert isinstance(q_law, QLaw) + assert isinstance(q_law, GuidanceLaw) + + def test_getters(self, q_law: QLaw): + assert q_law.get_parameters() is not None + assert q_law.get_target_coe() is not None + + def test_calculate_thrust_acceleration_at( + self, + q_law: QLaw, + position_coordinates: list[float], + velocity_coordinates: list[float], + thrust_acceleration: float, + instant: Instant, + frame: Frame, + ): + assert pytest.approx( + q_law.calculate_thrust_acceleration_at( + instant=instant, + position_coordinates=position_coordinates, + velocity_coordinates=velocity_coordinates, + thrust_acceleration=thrust_acceleration, + output_frame=frame, + ) + ) == np.array([0.0, 0.0033333320640941645, 2.9088817174504986e-06]) diff --git a/bindings/python/test/trajectory/orbit/models/kepler/test_coe.py b/bindings/python/test/trajectory/orbit/models/kepler/test_coe.py index 1d6d39c81..ab96e04f9 100644 --- a/bindings/python/test/trajectory/orbit/models/kepler/test_coe.py +++ b/bindings/python/test/trajectory/orbit/models/kepler/test_coe.py @@ -101,6 +101,9 @@ def test_getters( assert coe.get_orbital_period(Earth.EGM2008.gravitational_parameter) is not None assert coe.get_periapsis_radius() is not None assert coe.get_apoapsis_radius() is not None + assert coe.get_semi_latus_rectum() is not None + assert coe.get_radial_distance() is not None + assert coe.get_angular_momentum(Earth.EGM2008.gravitational_parameter) is not None assert coe.get_SI_vector(COE.AnomalyType.TrueAnomaly) is not None assert coe.get_SI_vector(COE.AnomalyType.MeanAnomaly) is not None assert coe.get_SI_vector(COE.AnomalyType.EccentricAnomaly) is not None @@ -120,6 +123,23 @@ def test_anomaly_conversions(self): is not None ) + def test_compute_methods(self): + semi_latus_rectum: float = COE.compute_semi_latus_rectum(7000.0e3, 0.0) + assert semi_latus_rectum is not None + assert ( + COE.compute_angular_momentum( + 7000.0e3, 0.0, Earth.EGM2008.gravitational_parameter + ) + is not None + ) + assert ( + COE.compute_angular_momentum( + semi_latus_rectum, Earth.EGM2008.gravitational_parameter + ) + is not None + ) + assert COE.compute_radial_distance(7000.0e3, 0.0, 0.0) is not None + def test_from_SI_vector( self, coe: COE, diff --git a/include/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/ConstantThrust.hpp b/include/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/ConstantThrust.hpp index 1a7c0ddf9..f152ab6b4 100644 --- a/include/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/ConstantThrust.hpp +++ b/include/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/ConstantThrust.hpp @@ -96,8 +96,8 @@ class ConstantThrust : public GuidanceLaw /// @brief Intrack Constant thrust dynamics /// - /// @param [in] velocityDirection A bool representing the direction of the thrust. Defaults to - /// true. + /// @param [in] velocityDirection A bool representing the direction of the thrust, with true + /// meaning along the velocity direction. Defaults to true. /// /// @return Constant Thrust dynamics diff --git a/include/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/QLaw.hpp b/include/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/QLaw.hpp new file mode 100644 index 000000000..026259f32 --- /dev/null +++ b/include/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/QLaw.hpp @@ -0,0 +1,173 @@ +/// Apache License 2.0 + +#ifndef __OpenSpaceToolkit_Astrodynamics_GuidanceLaw_QLaw__ +#define __OpenSpaceToolkit_Astrodynamics_GuidanceLaw_QLaw__ + +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include + +namespace ostk +{ +namespace astro +{ +namespace guidancelaw +{ + +using ostk::core::types::Real; +using ostk::core::types::Size; +using ostk::core::types::Shared; +using ostk::core::ctnr::Map; + +using ostk::math::obj::Vector3d; +using ostk::math::obj::Matrix3d; +using Vector5d = Eigen::Matrix; +using Matrix5d = Eigen::Matrix; +using Matrix53d = Eigen::Matrix; +using ostk::math::obj::Vector6d; +using ostk::math::obj::MatrixXd; + +using ostk::physics::time::Instant; +using ostk::physics::coord::Frame; +using ostk::physics::units::Derived; + +using ostk::astro::trajectory::orbit::models::kepler::COE; +using ostk::astro::GuidanceLaw; +using ostk::astro::solvers::FiniteDifferenceSolver; +using ostk::astro::trajectory::StateBuilder; + +/// @brief The Q-law is a Lyapunov feedback control law developed by Petropoulos, +/// based on analytic expressions for maximum rates of change of the orbit elements and +/// the desired changes in the elements. Q, the proximity quotient, serves as a candidate Lyapunov +/// function. As the spacecraft approaches the target orbit, Q decreases monotonically (becoming zero at the target +/// orbit). +/// @ref https://dataverse.jpl.nasa.gov/api/access/datafile/10307?gbrecs=true +/// @ref +/// https://www.researchgate.net/publication/370849580_Analytic_Calculation_and_Application_of_the_Q-Law_Guidance_Algorithm_Partial_Derivatives +/// @ref https://dataverse.jpl.nasa.gov/api/access/datafile/13727?gbrecs=true + +class QLaw : public GuidanceLaw +{ + public: + struct Parameters + { + Parameters( + const Map& anElementWeights, + const Size& aMValue = 3, + const Size& aNValue = 4, + const Size& aRValue = 2, + const Real& aBValue = 0.01 + ); + + Vector5d getControlWeights() const; + + const Size m; + const Size n; + const Size r; + const Real b; + + private: + Vector5d controlWeights_ = Vector5d::Zero(); + }; + + /// @brief Constructor + /// + /// @param [in] aCOE A target orbit described by Classical Orbital Elements. + /// @param [in] aGravitationalParameter The gravitational parameter of the central body. + /// @param [in] aParameterSet A set of parameters for the QLaw. + /// @param [in] aFiniteDifferenceSolver The finite difference solver. + + QLaw( + const COE& aCOE, + const Derived& aGravitationalParameter, + const Parameters& aParameterSet, + const FiniteDifferenceSolver& aFiniteDifferenceSolver + ); + + /// @brief Destructor + + virtual ~QLaw(); + + /// @brief Output stream operator + /// + /// @code + /// std::cout << QLaw(...) ; + /// @endcode + /// + /// @param [in] anOutputStream An output stream + /// @param [in] aGuidanceLaw A guidance Law + /// @return A reference to output stream + + friend std::ostream& operator<<(std::ostream& anOutputStream, const QLaw& aGuidanceLaw); + + /// @brief Get Parameters + /// + /// @return Parameters + + Parameters getParameters() const; + + /// @brief Get target COE + /// + /// @return target COE + + COE getTargetCOE() const; + + /// @brief Print guidance law + /// + /// @param [in] anOutputStream An output stream + /// @param [in] (optional) displayDecorators If true, display decorators + + virtual void print(std::ostream& anOutputStream, bool displayDecorator = true) const; + + /// @brief Compute the acceleration at the provided coordinates based on logic specific to the + /// guidance law + /// + /// @param [in] anInstant An instant + /// @param [in] aPositionCoordinates The position coordinates + /// @param [in] aVelocityCoordinates The velocity coordinates + /// @param [in] aThrustAcceleration The thrust acceleration + /// @param [in] outputFrame The output frame + /// + /// @return The acceleration at the provided coordinates + + virtual Vector3d calculateThrustAccelerationAt( + const Instant& anInstant, + const Vector3d& aPositionCoordinates, + const Vector3d& aVelocityCoordinates, + const Real& aThrustAcceleration, + const Shared& outputFrameSPtr + ) const override; + + private: + Parameters parameters_; + const Real mu_; + const Vector6d targetCOEVector_; + const Derived gravitationalParameter_; + const FiniteDifferenceSolver finiteDifferenceSolver_; + const StateBuilder stateBuilder_; + + Real computeQ(const Vector5d& currentCOEVector, const Real& aThrustAcceleration) const; + + Vector3d computeThrustDirection(const Vector6d& currentCOEVector, const Real& aThrustAcceleration) const; + + Matrix53d computeDOEWithF(const Vector6d& aCOEVector) const; + + Vector5d computeOrbitalElementsMaximalChange(const Vector5d& aCOEVector, const Real& aThrustAcceleration) const; + + static Matrix3d thetaRHToGCRF(const Vector3d& aPositionCoordinates, const Vector3d& aVelocityCoordinates); +}; + +} // namespace guidancelaw +} // namespace astro +} // namespace ostk + +#endif diff --git a/include/OpenSpaceToolkit/Astrodynamics/Trajectory/Orbit/Models/Kepler/COE.hpp b/include/OpenSpaceToolkit/Astrodynamics/Trajectory/Orbit/Models/Kepler/COE.hpp index d1e4eb073..82c9aa19b 100644 --- a/include/OpenSpaceToolkit/Astrodynamics/Trajectory/Orbit/Models/Kepler/COE.hpp +++ b/include/OpenSpaceToolkit/Astrodynamics/Trajectory/Orbit/Models/Kepler/COE.hpp @@ -62,8 +62,8 @@ class COE SemiMajorAxis, Eccentricity, Inclination, - Aop, Raan, + Aop, TrueAnomaly, MeanAnomaly, EccentricAnomaly @@ -186,6 +186,24 @@ class COE Length getApoapsisRadius() const; + /// @brief Get Semi-Latus Rectum + /// + /// @return Semi-Latus Rectum + + Length getSemiLatusRectum() const; + + /// @brief Get Radial Distance + /// + /// @return Radiaul Distance + + Length getRadialDistance() const; + + /// @brief Get Angular Momentum + /// + /// @return Angular Momentum + + Derived getAngularMomentum(const Derived& aGravitationalParameter) const; + /// @brief Get Mean motion /// /// @param [in] aGravitationalParameter A gravitational parameter @@ -226,7 +244,7 @@ class COE /// @brief Construct an undefined COE /// /// @code - /// COE coe = COE::Undefined() ; // Undefined + /// COE coe = COE::Undefined(); /// @endcode /// /// @return Undefined coe @@ -295,10 +313,45 @@ class COE const Angle& aMeanAnomly, const Real& anEccentricity, const Real& aTolerance ); + /// @brief Compute the semi-latus rectum of the orbit. + /// + /// @param [in] aSemiMajorAxis Semi-major axis of the orbit in meters. + /// @param [in] anEccentricity Eccentricity of the orbit. + /// @return Real Semi-latus rectum. + + static Real ComputeSemiLatusRectum(const Real& aSemiMajorAxis, const Real& anEccentricity); + + /// @brief Compute the angular momentum of the orbit. + /// + /// @param [in] aSemiMajorAxis Semi-major axis of the orbit in meters. + /// @param [in] anEccentricity Eccentricity of the orbit. + /// @param [in] aGravitationalParameter Gravitational parameter. + /// @return Angular momentum. + + static Real ComputeAngularMomentum( + const Real& aSemiMajorAxis, const Real& anEccentricity, const Derived& aGravitationalParameter + ); + + /// @brief Compute the angular momentum of the orbit. + /// + /// @param [in] aSemiLatusRectum Semi-latus rectum of the orbit in meters. + /// @param [in] aGravitationalParameter Gravitational parameter. + /// @return Angular momentum. + + static Real ComputeAngularMomentum(const Real& aSemiLatusRectum, const Derived& aGravitationalParameter); + + /// @brief Compute the radial distance at a given true anomaly. + /// + /// @param [in] aSemiMajorAxis Semi-major axis of the orbit in meters. + /// @param [in] anEccentricity Eccentricity of the orbit. + /// @param [in] trueAnomaly True anomaly in radians. + /// @return Radial distance. + + static Real ComputeRadialDistance(const Real& aSemiMajorAxis, const Real& anEccentricity, const Real& trueAnomaly); + /// @brief Convert element to string /// /// @param [in] anElement An element - /// /// @return String representing the element static String StringFromElement(const COE::Element& anElement); diff --git a/src/OpenSpaceToolkit/Astrodynamics/Dynamics/Thruster.cpp b/src/OpenSpaceToolkit/Astrodynamics/Dynamics/Thruster.cpp index 8b8932006..bda02ffbe 100644 --- a/src/OpenSpaceToolkit/Astrodynamics/Dynamics/Thruster.cpp +++ b/src/OpenSpaceToolkit/Astrodynamics/Dynamics/Thruster.cpp @@ -88,11 +88,11 @@ VectorXd Thruster::computeContribution( throw ostk::core::error::RuntimeError("Out of fuel."); } - const Real thrustAccelerationMagnitude = + const Real maximumThrustAccelerationMagnitude = satelliteSystem_.accessPropulsionSystem().getAcceleration(Mass::Kilograms(x[6])).getValue(); const Vector3d acceleration = guidanceLaw_->calculateThrustAccelerationAt( - anInstant, positionCoordinates, velocityCoordinates, thrustAccelerationMagnitude, aFrameSPtr + anInstant, positionCoordinates, velocityCoordinates, maximumThrustAccelerationMagnitude, aFrameSPtr ); // Compute contribution @@ -109,8 +109,8 @@ void Thruster::print(std::ostream& anOutputStream, bool displayDecorator) const displayDecorator ? ostk::core::utils::Print::Header(anOutputStream, "Thruster") : void(); ostk::core::utils::Print::Line(anOutputStream) << "Name:" << name_; - ostk::core::utils::Print::Line(anOutputStream) << "Satellite System:" << satelliteSystem_; - ostk::core::utils::Print::Line(anOutputStream) << "Guidance Law:" << guidanceLaw_; + satelliteSystem_.print(anOutputStream, false); + guidanceLaw_->print(anOutputStream, false); displayDecorator ? ostk::core::utils::Print::Footer(anOutputStream) : void(); } diff --git a/src/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/QLaw.cpp b/src/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/QLaw.cpp new file mode 100644 index 000000000..aed6f4d5c --- /dev/null +++ b/src/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/QLaw.cpp @@ -0,0 +1,348 @@ +/// Apache License 2.0 + +#include +#include + +#include +#include +#include + +namespace ostk +{ +namespace astro +{ +namespace guidancelaw +{ + +using ostk::math::obj::Vector6d; +using ostk::math::obj::VectorXd; + +using ostk::physics::coord::Position; +using ostk::physics::coord::Velocity; + +using ostk::astro::trajectory::State; +using ostk::astro::trajectory::state::CoordinatesSubset; + +QLaw::Parameters::Parameters( + const Map& anElementWeights, + const Size& aMValue, + const Size& aNValue, + const Size& aRValue, + const Real& aBValue +) + : m(aMValue), + n(aNValue), + r(aRValue), + b(aBValue) +{ + if (anElementWeights.count(COE::Element::TrueAnomaly)) + { + throw ostk::core::error::RuntimeError("Cannot target True Anomaly."); + } + + if (anElementWeights.count(COE::Element::MeanAnomaly)) + { + throw ostk::core::error::RuntimeError("Cannot target Mean Anomaly."); + } + + if (anElementWeights.count(COE::Element::EccentricAnomaly)) + { + throw ostk::core::error::RuntimeError("Cannot target Eccentric Anomaly."); + } + + if (anElementWeights.empty()) + { + throw ostk::core::error::RuntimeError("ElementWeights is empty. Must target at least one element."); + } + + if (anElementWeights.count(COE::Element::SemiMajorAxis)) + { + controlWeights_(0) = anElementWeights.at(COE::Element::SemiMajorAxis); + } + + if (anElementWeights.count(COE::Element::Eccentricity)) + { + controlWeights_(1) = anElementWeights.at(COE::Element::Eccentricity); + } + + if (anElementWeights.count(COE::Element::Inclination)) + { + controlWeights_(2) = anElementWeights.at(COE::Element::Inclination); + } + + if (anElementWeights.count(COE::Element::Raan)) + { + controlWeights_(3) = anElementWeights.at(COE::Element::Raan); + } + + if (anElementWeights.count(COE::Element::Aop)) + { + controlWeights_(4) = anElementWeights.at(COE::Element::Aop); + } +} + +Vector5d QLaw::Parameters::getControlWeights() const +{ + return controlWeights_; +} + +QLaw::QLaw( + const COE& aCOE, + const Derived& aGravitationalParameter, + const QLaw::Parameters& aParameterSet, + const FiniteDifferenceSolver& aFiniteDifferenceSolver +) + : GuidanceLaw("Q-Law"), + parameters_(aParameterSet), + mu_(aGravitationalParameter.in(aGravitationalParameter.getUnit())), + targetCOEVector_(aCOE.getSIVector(COE::AnomalyType::True)), + gravitationalParameter_(aGravitationalParameter), + finiteDifferenceSolver_(aFiniteDifferenceSolver), + stateBuilder_(Frame::GCRF(), {std::make_shared("QLaw Element Vector", 5)}) +{ +} + +QLaw::~QLaw() {} + +std::ostream& operator<<(std::ostream& anOutputStream, const QLaw& aGuidanceLaw) +{ + aGuidanceLaw.print(anOutputStream); + return anOutputStream; +} + +void QLaw::print(std::ostream& anOutputStream, bool displayDecorator) const +{ + displayDecorator ? ostk::core::utils::Print::Header(anOutputStream, "QLaw") : void(); + + displayDecorator ? ostk::core::utils::Print::Footer(anOutputStream) : void(); +} + +QLaw::Parameters QLaw::getParameters() const +{ + return parameters_; +} + +COE QLaw::getTargetCOE() const +{ + return COE::FromSIVector(targetCOEVector_, COE::AnomalyType::True); +} + +Vector3d QLaw::calculateThrustAccelerationAt( + [[maybe_unused]] const Instant& anInstant, + const Vector3d& aPositionCoordinates, + const Vector3d& aVelocityCoordinates, + const Real& aThrustAcceleration, + [[maybe_unused]] const Shared& outputFrameSPtr +) const +{ + const COE coe = COE::Cartesian( + {Position::Meters(aPositionCoordinates, Frame::GCRF()), + Velocity::MetersPerSecond(aVelocityCoordinates, Frame::GCRF())}, + gravitationalParameter_ + ); + + Vector6d coeVector = coe.getSIVector(COE::AnomalyType::True); + + static const double singularityTolerance = 1e-4; + coeVector[1] = std::max(coeVector[1], singularityTolerance); + coeVector[2] = std::max(singularityTolerance, std::min(coeVector[2], singularityTolerance)); + + const Vector3d thrustDirection = computeThrustDirection(coeVector, aThrustAcceleration); + + const Matrix3d R_thetaRH_GCRF = QLaw::thetaRHToGCRF(aPositionCoordinates, aVelocityCoordinates); + + return aThrustAcceleration * R_thetaRH_GCRF * thrustDirection; +} + +Vector3d QLaw::computeThrustDirection(const Vector6d& currentCOEVector, const Real& aThrustAcceleration) const +{ + const Matrix53d derivativeMatrix = computeDOEWithF(currentCOEVector); + + const auto getQ = [this, + &aThrustAcceleration](const State& aState, [[maybe_unused]] const Instant& anInstant) -> VectorXd + { + const Vector5d& coeVector = aState.accessCoordinates(); + + VectorXd coordinates(1); + coordinates << this->computeQ(coeVector, aThrustAcceleration); + + return coordinates; + }; + + const MatrixXd jacobian = finiteDifferenceSolver_.computeJacobian( + stateBuilder_.build(Instant::J2000(), currentCOEVector.segment(0, 5)), Instant::J2000(), getQ, 1 + ); + + const MatrixXd D = jacobian * derivativeMatrix; + + Vector3d thrustDirection = { + -D.col(0).sum(), + -D.col(1).sum(), + -D.col(2).sum(), + }; + + thrustDirection.normalize(); + + return thrustDirection; +} + +Real QLaw::computeQ(const Vector5d& currentCOEVector, const Real& aThrustAcceleration) const +{ + const Vector5d deltaCOE = { + (currentCOEVector[0] - targetCOEVector_[0]), + (currentCOEVector[1] - targetCOEVector_[1]), + (currentCOEVector[2] - targetCOEVector_[2]), + (std::acos(std::cos((currentCOEVector[3] - targetCOEVector_[3])))), + (std::acos(std::cos((currentCOEVector[4] - targetCOEVector_[4])))), + }; + + const Vector5d scalingCOE = { + std::pow( + (1.0 + std::pow(deltaCOE[0] / (parameters_.m * targetCOEVector_[0]), parameters_.n)), 1.0 / parameters_.r + ), + 1.0, + 1.0, + 1.0, + 1.0, + }; + + const Vector5d maximalCOE = computeOrbitalElementsMaximalChange(currentCOEVector, aThrustAcceleration); + + // TBI: Include Penalty function for minimum periapsis + + const Vector5d deltaCOE_divided_maximalCOE = (deltaCOE.cwiseQuotient(maximalCOE)); + + return (parameters_.getControlWeights() + .cwiseProduct(scalingCOE) + .cwiseProduct(deltaCOE_divided_maximalCOE.cwiseProduct(deltaCOE_divided_maximalCOE))) + .sum(); +} + +Vector5d QLaw::computeOrbitalElementsMaximalChange(const Vector5d& aCOEVector, const Real& aThrustAcceleration) const +{ + const Real& semiMajorAxis = aCOEVector[0]; + const Real& eccentricity = aCOEVector[1]; + const Real& inclination = aCOEVector[2]; + const Real& argumentOfPeriapsis = aCOEVector[4]; + + const Real semiLatusRectum = COE::ComputeSemiLatusRectum(semiMajorAxis, eccentricity); + const Real angularMomentum = COE::ComputeAngularMomentum(semiLatusRectum, gravitationalParameter_); + + // common grouped terms + const Real eccentricitySquared = eccentricity * eccentricity; + const Real aop_sin = std::sin(argumentOfPeriapsis); + const Real aop_cos = std::cos(argumentOfPeriapsis); + + // Semi-Major Axis + const Real semiMajorAxis_xx = + (2.0 * aThrustAcceleration * + std::sqrt(std::pow(semiMajorAxis, 3) * (1.0 + eccentricity) / (mu_ * (1.0 - eccentricity)))); + + // Eccentricity + const Real eccentricity_xx = 2.0 * semiLatusRectum * aThrustAcceleration / angularMomentum; + + // Inclination + const Real inclination_xx = (semiLatusRectum * aThrustAcceleration) / + (angularMomentum * std::sqrt(1.0 - (eccentricitySquared * aop_sin * aop_sin))) - + eccentricity * std::abs(aop_cos); + + // Right Ascension of the Ascending Node + const Real rightAscensionOfAscendingNode_xx = + (semiLatusRectum * aThrustAcceleration) / + (angularMomentum * std::sin(inclination) * + ((1.0 - (eccentricitySquared * aop_cos * aop_cos)) - eccentricity * std::abs(aop_sin))); + + // Argument of Periapsis + const Real alpha = (1.0 - eccentricitySquared) / (2.0 * std::pow(eccentricity, 3)); + const Real beta = std::sqrt(alpha * alpha + 1.0 / 27.0); + + const Real cosTheta_xx = + std::pow((alpha + beta), 1.0 / 3.0) - std::pow((beta - alpha), 1.0 / 3.0) - 1.0 / eccentricity; + const Real r_xx = semiLatusRectum / (1.0 + (eccentricity * cosTheta_xx)); + + const Real cosTheta_xxSquared = cosTheta_xx * cosTheta_xx; + + const Real argumentOfPeriapsisI_xx = (aThrustAcceleration / (eccentricity * angularMomentum)) * + std::sqrt( + semiLatusRectum * semiLatusRectum * cosTheta_xxSquared + + std::pow((semiLatusRectum + r_xx), 2) * (1.0 - cosTheta_xxSquared) + ); + const Real argumentOfPeriapsisO_xx = rightAscensionOfAscendingNode_xx * std::abs(std::cos(inclination)); + const Real argumentOfPeriapsis_xx = + (argumentOfPeriapsisI_xx + parameters_.b * argumentOfPeriapsisO_xx) / (1.0 + parameters_.b); + + return { + semiMajorAxis_xx, + eccentricity_xx, + inclination_xx, + rightAscensionOfAscendingNode_xx, + argumentOfPeriapsis_xx, + }; +} + +Matrix3d QLaw::thetaRHToGCRF(const Vector3d& aPositionCoordinates, const Vector3d& aVelocityCoordinates) +{ + const Vector3d R = aPositionCoordinates.normalized(); + const Vector3d H = aPositionCoordinates.cross(aVelocityCoordinates).normalized(); + const Vector3d theta = H.cross(R); + + Matrix3d rotationMatrix; + rotationMatrix.col(0) = theta; + rotationMatrix.col(1) = R; + rotationMatrix.col(2) = H; + + return rotationMatrix; +} + +Matrix53d QLaw::computeDOEWithF(const Vector6d& aCOEVector) const +{ + const Real& semiMajorAxis = aCOEVector[0]; + const Real& eccentricity = aCOEVector[1]; + const Real& inclination = aCOEVector[2]; + const Real& argumentOfPeriapsis = aCOEVector[4]; + const Real& trueAnomaly = aCOEVector[5]; + + const Real semiLatusRectum = COE::ComputeSemiLatusRectum(semiMajorAxis, eccentricity); + const Real angularMomentum = COE::ComputeAngularMomentum(semiLatusRectum, gravitationalParameter_); + const Real radialDistance = COE::ComputeRadialDistance(semiMajorAxis, eccentricity, trueAnomaly); + + // columns: Orbital elements + // rows: theta, radial, angular momentum directions + Matrix53d derivativeMatrix = Matrix53d::Zero(); + + // Common grouped operations + const Real trueAnomaly_sin = std::sin(trueAnomaly); + const Real trueAnomaly_cos = std::cos(trueAnomaly); + const Real trueAnomaly_ArgumentOfPeriapsis_sin = std::sin(trueAnomaly + argumentOfPeriapsis); + const Real trueAnomaly_ArgumentOfPeriapsis_cos = std::cos(trueAnomaly + argumentOfPeriapsis); + + // Semi-major axis + const Real sma_alpha = (2.0 * semiMajorAxis * semiMajorAxis / angularMomentum); + derivativeMatrix(0, 0) = sma_alpha * semiLatusRectum / radialDistance; + derivativeMatrix(0, 1) = sma_alpha * eccentricity * trueAnomaly_sin; + + // Eccentricity + derivativeMatrix(1, 0) = + ((semiLatusRectum + radialDistance) * trueAnomaly_cos + (radialDistance * eccentricity)) / angularMomentum; + derivativeMatrix(1, 1) = (semiLatusRectum * trueAnomaly_sin / angularMomentum); + + // Inclination + derivativeMatrix(2, 2) = (radialDistance * trueAnomaly_ArgumentOfPeriapsis_cos / angularMomentum); + + // Right Ascension of the Ascending Node + derivativeMatrix(3, 2) = + (radialDistance * trueAnomaly_ArgumentOfPeriapsis_sin) / (angularMomentum * std::sin(inclination)); + + // Argument of Periapsis + const Real aop_alpha = 1.0 / (eccentricity * angularMomentum); + derivativeMatrix(4, 0) = (semiLatusRectum + radialDistance) * trueAnomaly_sin * aop_alpha; + derivativeMatrix(4, 1) = -semiLatusRectum * trueAnomaly_cos * aop_alpha; + derivativeMatrix(4, 2) = (-radialDistance * trueAnomaly_ArgumentOfPeriapsis_sin * std::cos(inclination)) / + (angularMomentum * std::sin(inclination)); + + return derivativeMatrix; +} + +} // namespace guidancelaw +} // namespace astro +} // namespace ostk diff --git a/src/OpenSpaceToolkit/Astrodynamics/Trajectory/Orbit/Models/Kepler/COE.cpp b/src/OpenSpaceToolkit/Astrodynamics/Trajectory/Orbit/Models/Kepler/COE.cpp index 0a35df266..8c9bdf397 100644 --- a/src/OpenSpaceToolkit/Astrodynamics/Trajectory/Orbit/Models/Kepler/COE.cpp +++ b/src/OpenSpaceToolkit/Astrodynamics/Trajectory/Orbit/Models/Kepler/COE.cpp @@ -26,11 +26,26 @@ namespace kepler using ostk::physics::units::Derived; using ostk::physics::units::Length; using ostk::physics::units::Time; +using ostk::physics::units::Angle; +using ostk::physics::units::Mass; +using ostk::physics::units::ElectricCurrent; using EarthGravitationalModel = ostk::physics::environment::gravitational::Earth; static const Real Tolerance = 1e-30; static const Derived::Unit GravitationalParameterSIUnit = Derived::Unit::GravitationalParameter(Length::Unit::Meter, Time::Unit::Second); +static const Derived::Unit AngularMomentumSIUnit = { + Length::Unit::Meter, + {2}, + Mass::Unit::Kilogram, + {1}, + Time::Unit::Second, + {-2}, + ElectricCurrent::Unit::Undefined, + {0}, + Angle::Unit::Undefined, + {0}, +}; COE::COE( const Length& aSemiMajorAxis, @@ -180,6 +195,45 @@ Length COE::getApoapsisRadius() const return this->semiMajorAxis_ * (1.0 + this->eccentricity_); } +Length COE::getSemiLatusRectum() const +{ + if (!this->isDefined()) + { + throw ostk::core::error::runtime::Undefined("COE"); + } + + return Length::Meters(COE::ComputeSemiLatusRectum(semiMajorAxis_.inMeters(), eccentricity_)); +} + +Length COE::getRadialDistance() const +{ + if (!this->isDefined()) + { + throw ostk::core::error::runtime::Undefined("COE"); + } + + return Length::Meters(COE::ComputeRadialDistance( + semiMajorAxis_.inMeters(), + eccentricity_, + COE::ConvertAnomaly(anomaly_, eccentricity_, anomalyType_, AnomalyType::True, 1e-12).inRadians() + )); +} + +Derived COE::getAngularMomentum(const Derived& aGravitationalParameter) const +{ + if (!this->isDefined()) + { + throw ostk::core::error::runtime::Undefined("COE"); + } + + return Derived( + COE::ComputeAngularMomentum( + COE::ComputeSemiLatusRectum(semiMajorAxis_.inMeters(), eccentricity_), aGravitationalParameter + ), + AngularMomentumSIUnit + ); +} + Derived COE::getMeanMotion(const Derived& aGravitationalParameter) const { using ostk::physics::units::Mass; @@ -798,6 +852,32 @@ Angle COE::TrueAnomalyFromMeanAnomaly(const Angle& aMeanAnomly, const Real& anEc ); } +Real COE::ComputeSemiLatusRectum(const Real& aSemiMajorAxis, const Real& anEccentricity) +{ + return aSemiMajorAxis * (1.0 - (anEccentricity * anEccentricity)); +} + +Real COE::ComputeRadialDistance(const Real& aSemiMajorAxis, const Real& anEccentricity, const Real& aTrueAnomaly) +{ + return ComputeSemiLatusRectum(aSemiMajorAxis, anEccentricity) / (1.0 + anEccentricity * std::cos(aTrueAnomaly)); +} + +Real COE::ComputeAngularMomentum( + const Real& aSemiMajorAxis, const Real& anEccentricity, const Derived& aGravitationalParameter +) +{ + const Real mu_SI = aGravitationalParameter.in(GravitationalParameterSIUnit); + + return std::sqrt(mu_SI * ComputeSemiLatusRectum(aSemiMajorAxis, anEccentricity)); +} + +Real COE::ComputeAngularMomentum(const Real& aSemiLatusRectum, const Derived& aGravitationalParameter) +{ + const Real mu_SI = aGravitationalParameter.in(GravitationalParameterSIUnit); + + return std::sqrt(mu_SI * aSemiLatusRectum); +} + String COE::StringFromElement(const COE::Element& anElement) { switch (anElement) diff --git a/test/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/QLaw.test.cpp b/test/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/QLaw.test.cpp new file mode 100644 index 000000000..b27468a35 --- /dev/null +++ b/test/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/QLaw.test.cpp @@ -0,0 +1,155 @@ +/// Apache License 2.0 + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using ostk::core::types::Size; +using ostk::core::types::Real; +using ostk::core::types::Shared; +using ostk::core::ctnr::Array; + +using ostk::math::obj::Matrix3d; +using ostk::math::obj::Vector3d; +using ostk::math::obj::VectorXd; +using ostk::math::geom::d3::objects::Composite; +using ostk::math::geom::d3::objects::Cuboid; +using ostk::math::geom::d3::objects::Point; + +using ostk::physics::data::Scalar; +using ostk::physics::units::Derived; +using ostk::physics::units::Angle; +using ostk::physics::units::Length; +using ostk::physics::units::Mass; +using ostk::physics::time::Instant; +using ostk::physics::time::Duration; +using ostk::physics::coord::Frame; +using ostk::physics::env::obj::celest::Earth; +using EarthGravitationalModel = ostk::physics::environment::gravitational::Earth; + +using ostk::astro::guidancelaw::QLaw; +using ostk::astro::trajectory::State; +using ostk::astro::trajectory::Propagator; +using ostk::astro::trajectory::state::NumericalSolver; +using ostk::astro::trajectory::orbit::models::kepler::COE; +using ostk::astro::trajectory::state::CoordinatesSubset; +using ostk::astro::trajectory::state::CoordinatesBroker; +using ostk::astro::trajectory::state::coordinatessubsets::CartesianPosition; +using ostk::astro::trajectory::state::coordinatessubsets::CartesianVelocity; +using ostk::astro::solvers::FiniteDifferenceSolver; +using ostk::astro::flight::system::SatelliteSystem; +using ostk::astro::flight::system::PropulsionSystem; +using ostk::astro::Dynamics; +using ostk::astro::dynamics::CentralBodyGravity; +using ostk::astro::dynamics::Thruster; +using ostk::astro::dynamics::PositionDerivative; + +class OpenSpaceToolkit_Astrodynamics_Dynamics_Thruster_GuidanceLaw_QLaw : public ::testing::Test +{ + void SetUp() override + { + const COE currentCOE = { + Length::Meters(7000.0e3), + 0.01, + Angle::Degrees(0.05), + Angle::Degrees(0.0), + Angle::Degrees(0.0), + Angle::Degrees(0.0), + }; + + const COE::CartesianState cartesianState = currentCOE.getCartesianState(gravitationalParameter_, Frame::GCRF()); + + initialState_ = { + Instant::J2000(), + cartesianState.first, + cartesianState.second, + }; + } + + protected: + const QLaw::Parameters parameters_ = { + { + {COE::Element::SemiMajorAxis, 1.0}, + {COE::Element::Eccentricity, 1.0}, + }, + }; + + const COE targetCOE_ = { + Length::Meters(42000.0e3), + 0.01, + Angle::Degrees(0.05), + Angle::Degrees(0.0), + Angle::Degrees(0.0), + Angle::Degrees(0.0), + }; + + const Derived gravitationalParameter_ = + Derived(398600.49 * 1e9, EarthGravitationalModel::EGM2008.gravitationalParameter_.getUnit()); + + const Real thrustAcceleration_ = 1.0 / 300.0; + + const FiniteDifferenceSolver finiteDifferenceSolver_ = FiniteDifferenceSolver::Default(); + + const QLaw qlaw_ = { + targetCOE_, + gravitationalParameter_, + parameters_, + finiteDifferenceSolver_, + }; + + State initialState_ = State::Undefined(); +}; + +TEST_F(OpenSpaceToolkit_Astrodynamics_Dynamics_Thruster_GuidanceLaw_QLaw, Constructor) +{ + EXPECT_NO_THROW(QLaw qlaw(targetCOE_, gravitationalParameter_, parameters_, finiteDifferenceSolver_)); + + EXPECT_THROW( + QLaw qlaw(targetCOE_, gravitationalParameter_, {{}}, finiteDifferenceSolver_), ostk::core::error::RuntimeError + ); +} + +TEST_F(OpenSpaceToolkit_Astrodynamics_Dynamics_Thruster_GuidanceLaw_QLaw, GetParameters) +{ + EXPECT_NO_THROW(qlaw_.getParameters()); +} + +TEST_F(OpenSpaceToolkit_Astrodynamics_Dynamics_Thruster_GuidanceLaw_QLaw, GetTargetCOE) +{ + EXPECT_EQ(qlaw_.getTargetCOE(), targetCOE_); +} + +TEST_F(OpenSpaceToolkit_Astrodynamics_Dynamics_Thruster_GuidanceLaw_QLaw, CalculateThrustAccelerationAt) +{ + // Self-validated from the python code + { + const Vector3d acceleration = qlaw_.calculateThrustAccelerationAt( + initialState_.accessInstant(), + initialState_.getPosition().getCoordinates(), + initialState_.getVelocity().getCoordinates(), + thrustAcceleration_, + Frame::GCRF() + ); + + const Vector3d accelerationExpected = {0.0, 0.0033333320640941645, 2.9088817174504986e-06}; + + EXPECT_TRUE(acceleration.isNear(accelerationExpected, 1e-12)); + } +} diff --git a/test/OpenSpaceToolkit/Astrodynamics/Trajectory/Orbit/Models/Kepler/COE.test.cpp b/test/OpenSpaceToolkit/Astrodynamics/Trajectory/Orbit/Models/Kepler/COE.test.cpp index 16b70a0da..9d7a1c18b 100644 --- a/test/OpenSpaceToolkit/Astrodynamics/Trajectory/Orbit/Models/Kepler/COE.test.cpp +++ b/test/OpenSpaceToolkit/Astrodynamics/Trajectory/Orbit/Models/Kepler/COE.test.cpp @@ -344,6 +344,80 @@ TEST(OpenSpaceToolkit_Astrodynamics_Trajectory_Orbit_Models_Kepler_COE, GetApoap } } +TEST(OpenSpaceToolkit_Astrodynamics_Trajectory_Orbit_Models_Kepler_COE, GetSemiLatusRectum) +{ + { + const Length semiMajorAxis = Length::Kilometers(7000.0); + const Real eccentricity = 0.0; + const Angle inclination = Angle::Degrees(0.0); + const Angle raan = Angle::Degrees(0.0); + const Angle aop = Angle::Degrees(0.0); + const Angle trueAnomaly = Angle::Degrees(0.0); + + const COE coe = {semiMajorAxis, eccentricity, inclination, raan, aop, trueAnomaly}; + + EXPECT_DOUBLE_EQ( + semiMajorAxis.inMeters() * (1.0 - std::pow(eccentricity, 2)), coe.getSemiLatusRectum().inMeters() + ); + } + + { + EXPECT_ANY_THROW(COE::Undefined().getSemiLatusRectum()); + } +} + +TEST(OpenSpaceToolkit_Astrodynamics_Trajectory_Orbit_Models_Kepler_COE, GetRadialDistance) +{ + { + const Length semiMajorAxis = Length::Kilometers(7000.0); + const Real eccentricity = 0.0; + const Angle inclination = Angle::Degrees(0.0); + const Angle raan = Angle::Degrees(0.0); + const Angle aop = Angle::Degrees(0.0); + const Angle trueAnomaly = Angle::Degrees(0.0); + + const COE coe = {semiMajorAxis, eccentricity, inclination, raan, aop, trueAnomaly}; + + EXPECT_DOUBLE_EQ( + semiMajorAxis.inMeters() * (1.0 - std::pow(eccentricity, 2)) / + (1.0 + eccentricity * std::cos(trueAnomaly.inRadians())), + coe.getRadialDistance().inMeters() + ); + } + + { + EXPECT_ANY_THROW(COE::Undefined().getRadialDistance()); + } +} + +TEST(OpenSpaceToolkit_Astrodynamics_Trajectory_Orbit_Models_Kepler_COE, GetAngularMomentum) +{ + { + const Length semiMajorAxis = Length::Kilometers(7000.0); + const Real eccentricity = 0.0; + const Angle inclination = Angle::Degrees(0.0); + const Angle raan = Angle::Degrees(0.0); + const Angle aop = Angle::Degrees(0.0); + const Angle trueAnomaly = Angle::Degrees(0.0); + + const COE coe = {semiMajorAxis, eccentricity, inclination, raan, aop, trueAnomaly}; + + const Derived angularMomentum = coe.getAngularMomentum(Earth::EGM2008.gravitationalParameter_); + + EXPECT_DOUBLE_EQ( + std::sqrt( + Earth::EGM2008.gravitationalParameter_.in(Earth::EGM2008.gravitationalParameter_.getUnit()) * + coe.getSemiLatusRectum().inMeters() + ), + angularMomentum.in(angularMomentum.getUnit()) + ); + } + + { + EXPECT_ANY_THROW(COE::Undefined().getAngularMomentum(Earth::EGM2008.gravitationalParameter_)); + } +} + // TEST (OpenSpaceToolkit_Astrodynamics_Trajectory_Orbit_Models_Kepler_COE, GetMeanAnomaly) // { diff --git a/test/OpenSpaceToolkit/Astrodynamics/Trajectory/Propagator.test.cpp b/test/OpenSpaceToolkit/Astrodynamics/Trajectory/Propagator.test.cpp index 5081e1c11..a0059374a 100644 --- a/test/OpenSpaceToolkit/Astrodynamics/Trajectory/Propagator.test.cpp +++ b/test/OpenSpaceToolkit/Astrodynamics/Trajectory/Propagator.test.cpp @@ -45,8 +45,11 @@ #include #include #include +#include +#include #include #include +#include #include #include #include @@ -104,15 +107,15 @@ using ostk::physics::environment::atmospheric::earth::CSSISpaceWeather; using ostk::astro::eventcondition::InstantCondition; using ostk::astro::trajectory::State; using ostk::astro::trajectory::Propagator; +using ostk::astro::trajectory::LocalOrbitalFrameFactory; +using ostk::astro::trajectory::LocalOrbitalFrameDirection; using ostk::astro::trajectory::state::NumericalSolver; using ostk::astro::trajectory::state::CoordinatesSubset; using ostk::astro::trajectory::state::CoordinatesBroker; using ostk::astro::trajectory::state::coordinatessubsets::CartesianPosition; using ostk::astro::trajectory::state::coordinatessubsets::CartesianVelocity; using ostk::astro::trajectory::state::NumericalSolver; -using ostk::astro::trajectory::LocalOrbitalFrameFactory; -using ostk::astro::trajectory::LocalOrbitalFrameDirection; -using ostk::astro::trajectory::Propagator; +using ostk::astro::trajectory::orbit::models::kepler::COE; using ostk::astro::Dynamics; using ostk::astro::flight::system::PropulsionSystem; using ostk::astro::flight::system::SatelliteSystem; @@ -122,6 +125,8 @@ using ostk::astro::dynamics::ThirdBodyGravity; using ostk::astro::dynamics::AtmosphericDrag; using ostk::astro::dynamics::Thruster; using ostk::astro::guidancelaw::ConstantThrust; +using ostk::astro::guidancelaw::QLaw; +using ostk::astro::solvers::FiniteDifferenceSolver; static const Derived::Unit GravitationalParameterSIUnit = Derived::Unit::GravitationalParameter(Length::Unit::Meter, ostk::physics::units::Time::Unit::Second); @@ -2507,6 +2512,197 @@ class OpenSpaceToolkit_Astrodynamics_Trajectory_Orbit_Models_Propagator_Thruster Propagator defaultPropagator_ = Propagator::Undefined(); }; +TEST_F(OpenSpaceToolkit_Astrodynamics_Trajectory_Orbit_Models_Propagator, QLaw_Paper_Case_A) +{ + const COE targetCOE = { + Length::Meters(42000.0e3), + 0.01, + Angle::Degrees(0.05), + Angle::Degrees(0.0), + Angle::Degrees(0.0), + Angle::Degrees(0.0), + }; + + const QLaw::Parameters parameters = { + { + {COE::Element::SemiMajorAxis, 1.0}, + {COE::Element::Eccentricity, 1.0}, + }, + }; + + const Derived gravitationalParameter = + Derived(398600.49 * 1e9, EarthGravitationalModel::EGM2008.gravitationalParameter_.getUnit()); + + const Shared qlaw = + std::make_shared(QLaw(targetCOE, gravitationalParameter, parameters, FiniteDifferenceSolver::Default())); + + const Mass mass = Mass::Kilograms(50.0); + const Composite satelliteGeometry(Cuboid( + {0.0, 0.0, 0.0}, {Vector3d {1.0, 0.0, 0.0}, Vector3d {0.0, 1.0, 0.0}, Vector3d {0.0, 0.0, 1.0}}, {1.0, 2.0, 3.0} + )); + + const PropulsionSystem propulsionSystem = { + Scalar(1.0, PropulsionSystem::thrustSIUnit), + Scalar(3100.0, PropulsionSystem::specificImpulseSIUnit), + }; + + const SatelliteSystem satelliteSystem = { + mass, + satelliteGeometry, + Matrix3d::Identity(), + 1.0, + 2.1, + propulsionSystem, + }; + + const Shared thruster = std::make_shared(Thruster(satelliteSystem, qlaw)); + const Shared centralBodyGravity = + std::make_shared(CentralBodyGravity(std::make_shared(Earth::Spherical()))); + const Shared positionDerivative = std::make_shared(PositionDerivative()); + const Array> dynamics = { + thruster, + centralBodyGravity, + positionDerivative, + }; + + const NumericalSolver numericalSolver = { + NumericalSolver::LogType::NoLog, + NumericalSolver::StepperType::RungeKutta4, + 120.0, + 1e-12, + 1e-12, + }; + + const Propagator propagator = {numericalSolver, dynamics}; + + const COE currentCOE = { + Length::Meters(7000.0e3), + 0.01, + Angle::Degrees(0.05), + Angle::Degrees(0.0), + Angle::Degrees(0.0), + Angle::Degrees(0.0), + }; + + const COE::CartesianState cartesianState = currentCOE.getCartesianState(gravitationalParameter, Frame::GCRF()); + + VectorXd coordinates(7); + coordinates << cartesianState.first.getCoordinates(), cartesianState.second.getCoordinates(), 300.0; + + const State initialState = { + Instant::J2000(), + coordinates, + Frame::GCRF(), + {CartesianPosition::Default(), CartesianVelocity::Default(), CoordinatesSubset::Mass()} + }; + + const State state = propagator.calculateStateAt(initialState, initialState.accessInstant() + Duration::Days(14.82)); + + const COE endCOE = COE::Cartesian({state.getPosition(), state.getVelocity()}, gravitationalParameter); + + EXPECT_TRUE(std::abs(endCOE.getSemiMajorAxis().inMeters() - targetCOE.getSemiMajorAxis().inMeters()) < 60000.0); +} + +TEST_F(OpenSpaceToolkit_Astrodynamics_Trajectory_Orbit_Models_Propagator, QLaw_Paper_Case_E) +{ + const COE targetCOE = { + Length::Meters(26500.0e3), + 0.7, + Angle::Degrees(116.0), + Angle::Degrees(180.0), + Angle::Degrees(270.0), + Angle::Degrees(0.0), + }; + + const QLaw::Parameters parameters = { + { + {COE::Element::SemiMajorAxis, 1.0}, + {COE::Element::Eccentricity, 1.0}, + {COE::Element::Inclination, 1.0}, + {COE::Element::Raan, 1.0}, + {COE::Element::Aop, 1.0}, + }, + }; + + const Derived gravitationalParameter = + Derived(398600.49 * 1e9, EarthGravitationalModel::EGM2008.gravitationalParameter_.getUnit()); + + const Shared qlaw = + std::make_shared(QLaw(targetCOE, gravitationalParameter, parameters, FiniteDifferenceSolver::Default())); + + const Mass mass = Mass::Kilograms(50.0); + const Composite satelliteGeometry(Cuboid( + {0.0, 0.0, 0.0}, {Vector3d {1.0, 0.0, 0.0}, Vector3d {0.0, 1.0, 0.0}, Vector3d {0.0, 0.0, 1.0}}, {1.0, 2.0, 3.0} + )); + + const PropulsionSystem propulsionSystem = { + Scalar(2.0, PropulsionSystem::thrustSIUnit), + Scalar(2000.0, PropulsionSystem::specificImpulseSIUnit), + }; + + const SatelliteSystem satelliteSystem = { + mass, + satelliteGeometry, + Matrix3d::Identity(), + 1.0, + 2.1, + propulsionSystem, + }; + + const Shared thruster = std::make_shared(Thruster(satelliteSystem, qlaw)); + const Shared centralBodyGravity = + std::make_shared(CentralBodyGravity(std::make_shared(Earth::Spherical()))); + const Shared positionDerivative = std::make_shared(PositionDerivative()); + const Array> dynamics = { + thruster, + centralBodyGravity, + positionDerivative, + }; + + const NumericalSolver numericalSolver = { + NumericalSolver::LogType::NoLog, + NumericalSolver::StepperType::RungeKutta4, + 120.0, + 1e-12, + 1e-12, + }; + + const Propagator propagator = {numericalSolver, dynamics}; + + const COE currentCOE = { + Length::Meters(24505.9e3), + 0.725, + Angle::Degrees(0.06), + Angle::Degrees(0.0), + Angle::Degrees(0.0), + Angle::Degrees(0.0), + }; + + const COE::CartesianState cartesianState = currentCOE.getCartesianState(gravitationalParameter, Frame::GCRF()); + + VectorXd coordinates(7); + coordinates << cartesianState.first.getCoordinates(), cartesianState.second.getCoordinates(), 2000.0; + + const State initialState = { + Instant::J2000(), + coordinates, + Frame::GCRF(), + {CartesianPosition::Default(), CartesianVelocity::Default(), CoordinatesSubset::Mass()} + }; + + const State state = propagator.calculateStateAt(initialState, initialState.accessInstant() + Duration::Days(82.61)); + + const COE endCOE = COE::Cartesian({state.getPosition(), state.getVelocity()}, gravitationalParameter); + + EXPECT_TRUE(std::abs(endCOE.getSemiMajorAxis().inMeters() - targetCOE.getSemiMajorAxis().inMeters()) < 50000.0); + EXPECT_TRUE(std::abs(endCOE.getEccentricity() - targetCOE.getEccentricity()) < 1e-2); + + // TBI: These don't close yes, have to investigate why + // EXPECT_TRUE(std::abs(endCOE.getInclination().inDegrees() - targetCOE.getInclination().inDegrees()) < 1.0); + // EXPECT_TRUE(std::abs(endCOE.getRaan().inDegrees() - targetCOE.getRaan().inDegrees()) < 1.0); + // EXPECT_TRUE(std::abs(endCOE.getAop().inDegrees() - targetCOE.getAop().inDegrees()) < 1.0); +} + TEST_P( OpenSpaceToolkit_Astrodynamics_Trajectory_Orbit_Models_Propagator_Thruster, PropAccuracy_TwoBody_Newtonian_ConstantThrust diff --git a/test/OpenSpaceToolkit/Astrodynamics/Trajectory/Sequence.test.cpp b/test/OpenSpaceToolkit/Astrodynamics/Trajectory/Sequence.test.cpp index 121e6d68f..c24a10b8d 100644 --- a/test/OpenSpaceToolkit/Astrodynamics/Trajectory/Sequence.test.cpp +++ b/test/OpenSpaceToolkit/Astrodynamics/Trajectory/Sequence.test.cpp @@ -55,7 +55,6 @@ using EarthMagneticModel = ostk::physics::environment::magnetic::Earth; using EarthAtmosphericModel = ostk::physics::environment::atmospheric::Earth; using ostk::astro::trajectory::state::NumericalSolver; -using ostk::astro::Dynamics; using ostk::astro::flight::system::SatelliteSystem; using ostk::astro::flight::system::PropulsionSystem; using ostk::astro::trajectory::Segment; @@ -67,6 +66,7 @@ using ostk::astro::trajectory::state::CoordinatesBroker; using ostk::astro::trajectory::state::coordinatessubsets::CartesianPosition; using ostk::astro::trajectory::state::coordinatessubsets::CartesianVelocity; using ostk::astro::trajectory::orbit::models::kepler::COE; +using ostk::astro::Dynamics; using ostk::astro::dynamics::AtmosphericDrag; using ostk::astro::dynamics::CentralBodyGravity; using ostk::astro::dynamics::PositionDerivative;