From 89120196e4019373f411649dd0df894aa65db158 Mon Sep 17 00:00:00 2001 From: vishwa shah Date: Sun, 5 Jan 2025 03:02:49 +0000 Subject: [PATCH] feat!: rename computeJacobian to computeStateTransitionMatrix in FiniteDifferenceSolver class and add computeJacobian --- .../Solver/FiniteDifferenceSolver.cpp | 41 ++- .../solvers/test_finite_difference_solver.py | 21 +- .../Solver/FiniteDifferenceSolver.hpp | 22 +- .../Astrodynamics/GuidanceLaw/QLaw.cpp | 2 +- .../Solvers/FiniteDifferenceSolver.cpp | 101 +++++- .../Solver/FiniteDifferenceSolver.test.cpp | 304 ++++++++++++++++-- 6 files changed, 436 insertions(+), 55 deletions(-) diff --git a/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/Solver/FiniteDifferenceSolver.cpp b/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/Solver/FiniteDifferenceSolver.cpp index 2b068e523..28dc1515e 100644 --- a/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/Solver/FiniteDifferenceSolver.cpp +++ b/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/Solver/FiniteDifferenceSolver.cpp @@ -96,26 +96,28 @@ inline void OpenSpaceToolkitAstrodynamicsPy_Solver_FiniteDifferenceSolver(pybind ) .def( - "compute_jacobian", + "compute_state_transition_matrix", +[](const ostk::astrodynamics::solver::FiniteDifferenceSolver& solver, const State& aState, const Array& anInstantArray, const std::function&)>& generateStateCoordinates, const Size& aCoordinatesDimension) -> MatrixXd { - return solver.computeJacobian(aState, anInstantArray, generateStateCoordinates, aCoordinatesDimension); + return solver.computeStateTransitionMatrix( + aState, anInstantArray, generateStateCoordinates, aCoordinatesDimension + ); }, R"doc( - Compute the jacobian. + Compute the state transition matrix (STM). Args: state (State): The state. instants (Array(Instant)): The instants at which to calculate the STM. - generate_states_coordinates (function): The function to get the states. + generate_states_coordinates (callable): The function to get the states. coordinates_dimension (int): The dimension of the coordinates produced by `generate_states_coordinates`. Returns: - np.array: The jacobian. + np.array: The state transition matrix. )doc", arg("state"), arg("instants"), @@ -123,14 +125,16 @@ inline void OpenSpaceToolkitAstrodynamicsPy_Solver_FiniteDifferenceSolver(pybind arg("coordinates_dimension") ) .def( - "compute_jacobian", + "compute_state_transition_matrix", +[](const ostk::astrodynamics::solver::FiniteDifferenceSolver& solver, const State& aState, const Instant& anInstant, const std::function& generateStateCoordinates, const Size& aCoordinatesDimension) -> MatrixXd { - return solver.computeJacobian(aState, anInstant, generateStateCoordinates, aCoordinatesDimension); + return solver.computeStateTransitionMatrix( + aState, anInstant, generateStateCoordinates, aCoordinatesDimension + ); }, R"doc( Compute the jacobian. @@ -138,7 +142,7 @@ inline void OpenSpaceToolkitAstrodynamicsPy_Solver_FiniteDifferenceSolver(pybind Args: state (State): The state. instant (Instant): The instant at which to calculate the STM. - generate_state_coordinates (function): The function to get the state. + generate_state_coordinates (callable): The function to get the state. coordinates_dimension (int): The dimension of the coordinates produced by `generate_state_coordinates` Returns: @@ -160,6 +164,27 @@ inline void OpenSpaceToolkitAstrodynamicsPy_Solver_FiniteDifferenceSolver(pybind R"doc( Compute the gradient. + Args: + state (State): The state. + generate_state_coordinates (function): The function to generate the state coordinates. + + Returns: + np.array: The gradient. + )doc", + arg("state"), + arg("generate_state_coordinates") + ) + .def( + "compute_jacobian", + [](const ostk::astrodynamics::solver::FiniteDifferenceSolver& solver, + const State& aState, + std::function generateStateCoordinates) -> MatrixXd + { + return solver.computeJacobian(aState, generateStateCoordinates); + }, + R"doc( + Compute the jacobian. + Args: state (State): The state. generate_state_coordinates (function): The function to generate the state coordinates. diff --git a/bindings/python/test/solvers/test_finite_difference_solver.py b/bindings/python/test/solvers/test_finite_difference_solver.py index f25a43a19..bb6f6d79b 100644 --- a/bindings/python/test/solvers/test_finite_difference_solver.py +++ b/bindings/python/test/solvers/test_finite_difference_solver.py @@ -126,14 +126,14 @@ def test_string_from_type(self): == "Forward" ) - def test_compute_jacobian_array( + def test_compute_state_transition_matrix_array( self, finite_difference_solver: FiniteDifferenceSolver, state: State, instants: list[Instant], generate_states_coordinates: callable, ): - stm = finite_difference_solver.compute_jacobian( + stm = finite_difference_solver.compute_state_transition_matrix( state=state, instants=instants, generate_states_coordinates=generate_states_coordinates, @@ -145,14 +145,14 @@ def test_compute_jacobian_array( len(state.get_coordinates()), ) - def test_compute_jacobian_single( + def test_compute_state_transition_matrix_single( self, finite_difference_solver: FiniteDifferenceSolver, state: State, generate_state_coordinates: callable, instant: Instant, ): - stm = finite_difference_solver.compute_jacobian( + stm = finite_difference_solver.compute_state_transition_matrix( state=state, instant=instant, generate_state_coordinates=generate_state_coordinates, @@ -177,5 +177,18 @@ def test_compute_gradient( assert isinstance(gradient, np.ndarray) assert all(gradient - np.array([0.0, -1.0]) < 1e-6) + def test_compute_jacobian( + self, + finite_difference_solver: FiniteDifferenceSolver, + state: State, + generate_state_coordinates: callable, + ): + gradient = finite_difference_solver.compute_jacobian( + state=state, + generate_state_coordinates=generate_state_coordinates, + ) + assert isinstance(gradient, np.ndarray) + assert np.all(gradient - np.array([[0.0, 1.0], [-1.0, 0.0]]) < 1e-6) + def test_default(self): assert isinstance(FiniteDifferenceSolver.default(), FiniteDifferenceSolver) diff --git a/include/OpenSpaceToolkit/Astrodynamics/Solver/FiniteDifferenceSolver.hpp b/include/OpenSpaceToolkit/Astrodynamics/Solver/FiniteDifferenceSolver.hpp index 2c5ef9118..1a5dd1a69 100644 --- a/include/OpenSpaceToolkit/Astrodynamics/Solver/FiniteDifferenceSolver.hpp +++ b/include/OpenSpaceToolkit/Astrodynamics/Solver/FiniteDifferenceSolver.hpp @@ -83,7 +83,7 @@ class FiniteDifferenceSolver /// @return The step duration. Duration getStepDuration() const; - /// @brief Compute the Jacobian by perturbing the coordinates + /// @brief Compute the State Transition Matrix (STM) by perturbing the coordinates /// /// @param aState A state. /// @param anInstantArray An array of instants. @@ -92,15 +92,15 @@ class FiniteDifferenceSolver /// @param aCoordinatesDimension The dimension of the coordinates produced by /// `generateStateCoordinates`. /// - /// @return The Jacobian - MatrixXd computeJacobian( + /// @return The State Transition Matrix (STM) + MatrixXd computeStateTransitionMatrix( const State& aState, const Array& anInstantArray, const std::function&)>& generateStateCoordinates, const Size& aCoordinatesDimension ) const; - /// @brief Compute the Jacobian by perturbing the coordinates + /// @brief Compute the State Transition Matrix (STM) by perturbing the coordinates /// /// @param aState A state. /// @param anInstant An instant. @@ -108,8 +108,8 @@ class FiniteDifferenceSolver /// requested Instant. /// @param aCoordinatesDimension The dimension of the coordinates produced by /// `generateStateCoordinates`. - /// @return The Jacobian - MatrixXd computeJacobian( + /// @return The State Transition Matrix (STM) + MatrixXd computeStateTransitionMatrix( const State& aState, const Instant& anInstant, const std::function& generateStateCoordinates, @@ -127,6 +127,16 @@ class FiniteDifferenceSolver const State& aState, const std::function& generateStateCoordinates ) const; + /// @brief Compute the Jacobian. + /// + /// @param aState The state to compute the Jacobian of. + /// @param generateStateDerivatives Callable to generate derivatives of the State. + /// + /// @return The Jacobian. + MatrixXd computeJacobian( + const State& aState, const std::function& generateStateDerivatives + ) const; + /// @brief Print the solver. /// /// @param anOutputStream An output stream. diff --git a/src/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/QLaw.cpp b/src/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/QLaw.cpp index 65ea03159..52918843f 100644 --- a/src/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/QLaw.cpp +++ b/src/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/QLaw.cpp @@ -659,7 +659,7 @@ Vector5d QLaw::computeNumerical_dQ_dOE(const Vector5d& aCOEVector, const double& const Vector5d jacobian = Eigen::Map( finiteDifferenceSolver_ - .computeJacobian(stateBuilder_.build(Instant::J2000(), aCOEVector), Instant::J2000(), getQ, 1) + .computeStateTransitionMatrix(stateBuilder_.build(Instant::J2000(), aCOEVector), Instant::J2000(), getQ, 1) .data(), 5 ); diff --git a/src/OpenSpaceToolkit/Astrodynamics/Solvers/FiniteDifferenceSolver.cpp b/src/OpenSpaceToolkit/Astrodynamics/Solvers/FiniteDifferenceSolver.cpp index 66ab3ec7d..4afb33ccc 100644 --- a/src/OpenSpaceToolkit/Astrodynamics/Solvers/FiniteDifferenceSolver.cpp +++ b/src/OpenSpaceToolkit/Astrodynamics/Solvers/FiniteDifferenceSolver.cpp @@ -50,7 +50,7 @@ Duration FiniteDifferenceSolver::getStepDuration() const return stepDuration_; } -MatrixXd FiniteDifferenceSolver::computeJacobian( +MatrixXd FiniteDifferenceSolver::computeStateTransitionMatrix( const State& aState, const Array& anInstantArray, const std::function&)>& generateStateCoordinates, @@ -138,7 +138,7 @@ MatrixXd FiniteDifferenceSolver::computeJacobian( ? aState.accessCoordinates()(i) * stepPercentage_ : stepPercentage_; - MatrixXd differencedCoordinates = computeBlock(stepSize, aState.getCoordinates(), i); + MatrixXd differencedCoordinates = computeBlock(stepSize, aState.accessCoordinates(), i); const VectorXd columnStackedCoordinates = Eigen::Map(differencedCoordinates.data(), differencedCoordinates.size()); @@ -149,7 +149,7 @@ MatrixXd FiniteDifferenceSolver::computeJacobian( return A; } -MatrixXd FiniteDifferenceSolver::computeJacobian( +MatrixXd FiniteDifferenceSolver::computeStateTransitionMatrix( const State& aState, const Instant& anInstant, const std::function& generateStateCoordinates, @@ -167,7 +167,7 @@ MatrixXd FiniteDifferenceSolver::computeJacobian( return coordinatesMatrix; }; - return computeJacobian(aState, {anInstant}, generateStatesCoordinates, aCoordinatesDimension); + return computeStateTransitionMatrix(aState, {anInstant}, generateStatesCoordinates, aCoordinatesDimension); } VectorXd FiniteDifferenceSolver::computeGradient( @@ -178,7 +178,7 @@ VectorXd FiniteDifferenceSolver::computeGradient( { case FiniteDifferenceSolver::Type::Forward: { - const VectorXd coordinates = generateStateCoordinates(aState, aState.accessInstant()); + const VectorXd& coordinates = aState.accessCoordinates(); const Instant instant = aState.accessInstant() + stepDuration_; const VectorXd forwardCoordinates = generateStateCoordinates(aState, instant); @@ -190,7 +190,7 @@ VectorXd FiniteDifferenceSolver::computeGradient( case FiniteDifferenceSolver::Type::Backward: { - const VectorXd coordinates = generateStateCoordinates(aState, aState.accessInstant()); + const VectorXd& coordinates = aState.accessCoordinates(); const Instant instant = aState.accessInstant() - stepDuration_; const VectorXd backwardCoordinates = generateStateCoordinates(aState, instant); @@ -216,6 +216,95 @@ VectorXd FiniteDifferenceSolver::computeGradient( } } +MatrixXd FiniteDifferenceSolver::computeJacobian( + const State& aState, const std::function& generateStateCoordinates +) const +{ + const Size stateVectorDimension = aState.getSize(); + const VectorXd baseCoordinates = generateStateCoordinates(aState, aState.accessInstant()); + const Size coordinatesDimension = baseCoordinates.size(); + + MatrixXd jacobian = MatrixXd::Zero(coordinatesDimension, stateVectorDimension); + + const StateBuilder stateBuilder = {aState}; + + std::function computeBlock; + + switch (type_) + { + case FiniteDifferenceSolver::Type::Forward: + computeBlock = [&generateStateCoordinates, &stateBuilder, &aState, this]( + const Real& aStepSize, const Index& anIndex + ) -> VectorXd + { + VectorXd perturbedCoordinates = aState.accessCoordinates(); + + perturbedCoordinates(anIndex) += aStepSize; + const VectorXd forwardDerivatives = computeGradient( + stateBuilder.build(aState.accessInstant(), perturbedCoordinates), generateStateCoordinates + ); + + const VectorXd currentDerivatives = computeGradient(aState, generateStateCoordinates); + + return (forwardDerivatives - currentDerivatives) / aStepSize; + }; + break; + + case FiniteDifferenceSolver::Type::Backward: + computeBlock = [&generateStateCoordinates, &stateBuilder, &aState, this]( + const Real& aStepSize, const Index& anIndex + ) -> VectorXd + { + VectorXd perturbedCoordinates = aState.accessCoordinates(); + + perturbedCoordinates(anIndex) -= aStepSize; + const VectorXd backwardDerivatives = computeGradient( + stateBuilder.build(aState.accessInstant(), perturbedCoordinates), generateStateCoordinates + ); + + const VectorXd currentDerivatives = computeGradient(aState, generateStateCoordinates); + + return (currentDerivatives - backwardDerivatives) / aStepSize; + }; + break; + + case FiniteDifferenceSolver::Type::Central: + computeBlock = [&generateStateCoordinates, &stateBuilder, &aState, this]( + const Real& aStepSize, const Index& anIndex + ) -> VectorXd + { + VectorXd perturbedCoordinates = aState.accessCoordinates(); + + perturbedCoordinates(anIndex) += aStepSize; + const VectorXd forwardDerivatives = computeGradient( + stateBuilder.build(aState.accessInstant(), perturbedCoordinates), generateStateCoordinates + ); + + perturbedCoordinates(anIndex) -= 2.0 * aStepSize; + const VectorXd backwardDerivatives = computeGradient( + stateBuilder.build(aState.accessInstant(), perturbedCoordinates), generateStateCoordinates + ); + + return (forwardDerivatives - backwardDerivatives) / (2.0 * aStepSize); + }; + break; + + default: + throw ostk::core::error::runtime::Wrong("Finite Difference Solver Type."); + } + + for (Index i = 0; i < stateVectorDimension; ++i) + { + const Real stepSize = (aState.accessCoordinates()(i) * stepPercentage_ != 0.0) + ? aState.accessCoordinates()(i) * stepPercentage_ + : stepPercentage_; + + jacobian.col(i) = computeBlock(stepSize, i); + } + + return jacobian; +} + void FiniteDifferenceSolver::print(std::ostream& anOutputStream, bool displayDecorator) const { if (displayDecorator) diff --git a/test/OpenSpaceToolkit/Astrodynamics/Solver/FiniteDifferenceSolver.test.cpp b/test/OpenSpaceToolkit/Astrodynamics/Solver/FiniteDifferenceSolver.test.cpp index e60694bab..2935127f8 100644 --- a/test/OpenSpaceToolkit/Astrodynamics/Solver/FiniteDifferenceSolver.test.cpp +++ b/test/OpenSpaceToolkit/Astrodynamics/Solver/FiniteDifferenceSolver.test.cpp @@ -4,8 +4,17 @@ #include +#include +#include +#include +#include +#include + #include +#include #include +#include +#include #include using ostk::core::container::Array; @@ -13,22 +22,30 @@ using ostk::core::type::Real; using ostk::core::type::Shared; using ostk::core::type::Size; +using ostk::mathematics::object::MatrixXd; +using ostk::mathematics::object::Vector3d; +using ostk::mathematics::object::VectorXd; + using ostk::physics::coordinate::Frame; +using ostk::physics::Environment; +using ostk::physics::environment::object::celestial::Earth; +using EarthGravitationalModel = ostk::physics::environment::gravitational::Earth; using ostk::physics::time::Duration; using ostk::physics::time::Instant; - -using ostk::mathematics::object::MatrixXd; -using ostk::mathematics::object::VectorXd; +using ostk::physics::unit::Derived; using ostk::astrodynamics::solver::FiniteDifferenceSolver; +using ostk::astrodynamics::trajectory::Propagator; using ostk::astrodynamics::trajectory::State; using ostk::astrodynamics::trajectory::state::CoordinateSubset; +using ostk::astrodynamics::trajectory::state::coordinatesubset::CartesianPosition; +using ostk::astrodynamics::trajectory::state::coordinatesubset::CartesianVelocity; class OpenSpaceToolkit_Astrodynamics_Solvers_FiniteDifferenceSolver : public ::testing::Test { void SetUp() override { - generateStatesCoordinates = [](const State& aState, const Array& anInstantArray) -> MatrixXd + generateStatesCoordinates_ = [](const State& aState, const Array& anInstantArray) -> MatrixXd { MatrixXd statesCoordinates(2, anInstantArray.size()); @@ -53,7 +70,7 @@ class OpenSpaceToolkit_Astrodynamics_Solvers_FiniteDifferenceSolver : public ::t return statesCoordinates; }; - generateStateCoordinates = [](const State& aState, const Instant& anInstant) -> VectorXd + generateStateCoordinates_ = [](const State& aState, const Instant& anInstant) -> VectorXd { const Real& x0 = aState.accessCoordinates()(0); // Initial position const Real& v0 = aState.accessCoordinates()(1); // Initial position @@ -78,7 +95,7 @@ class OpenSpaceToolkit_Astrodynamics_Solvers_FiniteDifferenceSolver : public ::t protected: const FiniteDifferenceSolver::Type defaultType_ = FiniteDifferenceSolver::Type::Central; const Real defaultStepPercentage_ = 1e-3; - const Duration defaultStepDuration_ = Duration::Milliseconds(1e-3); + const Duration defaultStepDuration_ = Duration::Microseconds(1.0); const FiniteDifferenceSolver defaultSolver_ = { defaultType_, defaultStepPercentage_, @@ -92,8 +109,8 @@ class OpenSpaceToolkit_Astrodynamics_Solvers_FiniteDifferenceSolver : public ::t State initialState_ = State::Undefined(); - std::function&)> generateStatesCoordinates; - std::function generateStateCoordinates; + std::function&)> generateStatesCoordinates_; + std::function generateStateCoordinates_; }; TEST_F(OpenSpaceToolkit_Astrodynamics_Solvers_FiniteDifferenceSolver, Constructor) @@ -127,7 +144,7 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Solvers_FiniteDifferenceSolver, Getters) } } -TEST_F(OpenSpaceToolkit_Astrodynamics_Solvers_FiniteDifferenceSolver, ComputeJacobian) +TEST_F(OpenSpaceToolkit_Astrodynamics_Solvers_FiniteDifferenceSolver, computeStateTransitionMatrix) { { const Array instants = { @@ -136,11 +153,11 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Solvers_FiniteDifferenceSolver, ComputeJac Instant::J2000() + Duration::Seconds(300.0), }; - MatrixXd expectedJacobian(2 * instants.getSize(), 2); + MatrixXd expectedStateTransitionMatrix(2 * instants.getSize(), 2); - expectedJacobian << std::cos(100.0), std::sin(100.0), std::sin(-100.0), std::cos(100.0), std::cos(200.0), - std::sin(200.0), std::sin(-200.0), std::cos(200.0), std::cos(300.0), std::sin(300.0), std::sin(-300.0), - std::cos(300.0); + expectedStateTransitionMatrix << std::cos(100.0), std::sin(100.0), std::sin(-100.0), std::cos(100.0), + std::cos(200.0), std::sin(200.0), std::sin(-200.0), std::cos(200.0), std::cos(300.0), std::sin(300.0), + std::sin(-300.0), std::cos(300.0); { const FiniteDifferenceSolver solver = { @@ -148,9 +165,10 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Solvers_FiniteDifferenceSolver, ComputeJac defaultStepPercentage_, defaultStepDuration_, }; - const MatrixXd jacobian = solver.computeJacobian(initialState_, instants, generateStatesCoordinates, 2); + const MatrixXd jacobian = + solver.computeStateTransitionMatrix(initialState_, instants, generateStatesCoordinates_, 2); - EXPECT_TRUE(jacobian.isApprox(expectedJacobian, 1e-12)); + EXPECT_TRUE(jacobian.isApprox(expectedStateTransitionMatrix, 1e-12)); } { @@ -159,8 +177,9 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Solvers_FiniteDifferenceSolver, ComputeJac defaultStepPercentage_, defaultStepDuration_, }; - const MatrixXd jacobian = solver.computeJacobian(initialState_, instants, generateStatesCoordinates, 2); - EXPECT_TRUE(jacobian.isApprox(expectedJacobian, 1e-12)); + const MatrixXd jacobian = + solver.computeStateTransitionMatrix(initialState_, instants, generateStatesCoordinates_, 2); + EXPECT_TRUE(jacobian.isApprox(expectedStateTransitionMatrix, 1e-12)); } { @@ -169,16 +188,17 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Solvers_FiniteDifferenceSolver, ComputeJac defaultStepPercentage_, defaultStepDuration_, }; - const MatrixXd jacobian = solver.computeJacobian(initialState_, instants, generateStatesCoordinates, 2); - EXPECT_TRUE(jacobian.isApprox(expectedJacobian, 1e-12)); + const MatrixXd jacobian = + solver.computeStateTransitionMatrix(initialState_, instants, generateStatesCoordinates_, 2); + EXPECT_TRUE(jacobian.isApprox(expectedStateTransitionMatrix, 1e-12)); } } { const Instant instant = Instant::J2000() + Duration::Seconds(100.0); - MatrixXd expectedJacobian(2, 2); - expectedJacobian << std::cos(100.0), std::sin(100.0), std::sin(-100.0), std::cos(100.0); + MatrixXd expectedStateTransitionMatrix(2, 2); + expectedStateTransitionMatrix << std::cos(100.0), std::sin(100.0), std::sin(-100.0), std::cos(100.0); { const FiniteDifferenceSolver solver = { @@ -186,9 +206,10 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Solvers_FiniteDifferenceSolver, ComputeJac defaultStepPercentage_, defaultStepDuration_, }; - const MatrixXd jacobian = solver.computeJacobian(initialState_, instant, generateStateCoordinates, 2); + const MatrixXd stateTransitionMatrix = + solver.computeStateTransitionMatrix(initialState_, instant, generateStateCoordinates_, 2); - EXPECT_TRUE(jacobian.isApprox(expectedJacobian, 1e-12)); + EXPECT_TRUE(stateTransitionMatrix.isApprox(expectedStateTransitionMatrix, 1e-12)); } { @@ -197,9 +218,10 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Solvers_FiniteDifferenceSolver, ComputeJac defaultStepPercentage_, defaultStepDuration_, }; - const MatrixXd jacobian = solver.computeJacobian(initialState_, instant, generateStateCoordinates, 2); + const MatrixXd stateTransitionMatrix = + solver.computeStateTransitionMatrix(initialState_, instant, generateStateCoordinates_, 2); - EXPECT_TRUE(jacobian.isApprox(expectedJacobian, 1e-12)); + EXPECT_TRUE(stateTransitionMatrix.isApprox(expectedStateTransitionMatrix, 1e-12)); } { @@ -208,9 +230,10 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Solvers_FiniteDifferenceSolver, ComputeJac defaultStepPercentage_, defaultStepDuration_, }; - const MatrixXd jacobian = solver.computeJacobian(initialState_, instant, generateStateCoordinates, 2); + const MatrixXd stateTransitionMatrix = + solver.computeStateTransitionMatrix(initialState_, instant, generateStateCoordinates_, 2); - EXPECT_TRUE(jacobian.isApprox(expectedJacobian, 1e-12)); + EXPECT_TRUE(stateTransitionMatrix.isApprox(expectedStateTransitionMatrix, 1e-12)); } } } @@ -226,7 +249,7 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Solvers_FiniteDifferenceSolver, ComputeGra defaultStepPercentage_, defaultStepDuration_, }; - const VectorXd gradient = solver.computeGradient(initialState_, generateStateCoordinates); + const VectorXd gradient = solver.computeGradient(initialState_, generateStateCoordinates_); EXPECT_TRUE(gradient.isApprox(expectedGradient, 1e-6)); } @@ -237,7 +260,7 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Solvers_FiniteDifferenceSolver, ComputeGra defaultStepPercentage_, defaultStepDuration_, }; - const VectorXd gradient = solver.computeGradient(initialState_, generateStateCoordinates); + const VectorXd gradient = solver.computeGradient(initialState_, generateStateCoordinates_); EXPECT_TRUE(gradient.isApprox(expectedGradient, 1e-6)); } @@ -248,7 +271,87 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Solvers_FiniteDifferenceSolver, ComputeGra defaultStepPercentage_, defaultStepDuration_, }; - const VectorXd gradient = solver.computeGradient(initialState_, generateStateCoordinates); + const VectorXd gradient = solver.computeGradient(initialState_, generateStateCoordinates_); + + EXPECT_TRUE(gradient.isApprox(expectedGradient, 1e-6)); + } +} + +TEST_F(OpenSpaceToolkit_Astrodynamics_Solvers_FiniteDifferenceSolver, ComputeGradient_TwoBodyProblem) +{ + VectorXd coordinates(6); + coordinates << 7000000.0, 0.0, 0.0, 0.0, 7546.0, 0.0; + + const State state = { + Instant::J2000(), + coordinates, + Frame::GCRF(), + {CartesianPosition::Default(), CartesianVelocity::Default()}, + }; + + const Environment environment = Environment(Instant::J2000(), {std::make_shared(Earth::Spherical())}); + + const Propagator propagator = Propagator::Default(environment); + + const auto generateStateCoordinates = [&propagator](const State& aState, const Instant& anInstant) -> VectorXd + { + return propagator.calculateStateAt(aState, anInstant).accessCoordinates(); + }; + + const Real mu = + EarthGravitationalModel::EGM2008.gravitationalParameter_.in(Derived::Unit::MeterCubedPerSecondSquared()); + + const Vector3d positionCoordinates = state.getPosition().getCoordinates(); + const Real r = positionCoordinates.norm(); + const Real r3 = std::pow(r, 3); + + const Real x = positionCoordinates(0); + const Real y = positionCoordinates(1); + const Real z = positionCoordinates(2); + + VectorXd expectedGradient(6); + expectedGradient << coordinates(3), coordinates(4), coordinates(5), -mu * x / r3, -mu * y / r3, -mu * z / r3; + + { + const FiniteDifferenceSolver solver = { + FiniteDifferenceSolver::Type::Central, + defaultStepPercentage_, + defaultStepDuration_, + }; + const VectorXd gradient = solver.computeGradient(state, generateStateCoordinates); + + EXPECT_TRUE(gradient.isApprox(expectedGradient, 1e-6)); + } + + { + const FiniteDifferenceSolver solver = { + FiniteDifferenceSolver::Type::Forward, + defaultStepPercentage_, + defaultStepDuration_, + }; + const VectorXd gradient = solver.computeGradient(state, generateStateCoordinates); + + EXPECT_TRUE(gradient.isApprox(expectedGradient, 1e-6)); + } + + { + const FiniteDifferenceSolver solver = { + FiniteDifferenceSolver::Type::Backward, + defaultStepPercentage_, + defaultStepDuration_, + }; + const VectorXd gradient = solver.computeGradient(state, generateStateCoordinates); + + EXPECT_TRUE(gradient.isApprox(expectedGradient, 1e-6)); + } + + { + const FiniteDifferenceSolver solver = { + FiniteDifferenceSolver::Type::Central, + defaultStepPercentage_, + defaultStepDuration_, + }; + const VectorXd gradient = solver.computeGradient(state, generateStateCoordinates); EXPECT_TRUE(gradient.isApprox(expectedGradient, 1e-6)); } @@ -277,3 +380,144 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Solvers_FiniteDifferenceSolver, Default) { EXPECT_NO_THROW(FiniteDifferenceSolver::Default()); } + +TEST_F(OpenSpaceToolkit_Astrodynamics_Solvers_FiniteDifferenceSolver, ComputeJacobian) +{ + const State state = { + Instant::J2000(), + VectorXd::Zero(2), + Frame::GCRF(), + {std::make_shared("Position", 1), std::make_shared("Velocity", 1)}, + }; + + const auto generateStateCoordinates = [](const State& aState, const Instant& anInstant) -> VectorXd + { + const Real& x0 = aState.accessCoordinates()(0); // Initial position + const Real& v0 = aState.accessCoordinates()(1); // Initial position + const Real omega = std::sqrt(1.0); // Angular frequency, assuming unit mass and spring constant + + const Real t = (anInstant - aState.accessInstant()).inSeconds(); + const Real x = x0 * std::cos(omega * t) + v0 / omega * std::sin(omega * t); + const Real v = -x0 * omega * std::sin(omega * t) + v0 * std::cos(omega * t); + + VectorXd coordinates(2, 1); + coordinates << x, v; + + return coordinates; + }; + + MatrixXd expectedJacobian(2, 2); + expectedJacobian << 0.0, 1.0, -1.0, 0.0; + + { + const FiniteDifferenceSolver solver = { + FiniteDifferenceSolver::Type::Central, + defaultStepPercentage_, + defaultStepDuration_, + }; + const MatrixXd jacobian = solver.computeJacobian(state, generateStateCoordinates); + + EXPECT_TRUE(jacobian.isApprox(expectedJacobian, 1e-6)); + } + + { + const FiniteDifferenceSolver solver = { + FiniteDifferenceSolver::Type::Forward, + defaultStepPercentage_, + defaultStepDuration_, + }; + const MatrixXd jacobian = solver.computeJacobian(state, generateStateCoordinates); + + EXPECT_TRUE(jacobian.isApprox(expectedJacobian, 1e-6)); + } + + { + const FiniteDifferenceSolver solver = { + FiniteDifferenceSolver::Type::Backward, + defaultStepPercentage_, + defaultStepDuration_, + }; + const MatrixXd jacobian = solver.computeJacobian(state, generateStateCoordinates); + + EXPECT_TRUE(jacobian.isApprox(expectedJacobian, 1e-6)); + } +} + +TEST_F(OpenSpaceToolkit_Astrodynamics_Solvers_FiniteDifferenceSolver, ComputeJacobian_TwoBodyProblem) +{ + VectorXd coordinates(6); + coordinates << -71095.1731570421, -4419328.15020176, -5283063.76725156, -1480.1545494292, 5721.69282426838, + -4772.2461992043; + + const State state = { + Instant::J2000(), + coordinates, + Frame::GCRF(), + {CartesianPosition::Default(), CartesianVelocity::Default()}, + }; + + const Environment environment = Environment(Instant::J2000(), {std::make_shared(Earth::Spherical())}); + + const Propagator propagator = Propagator::Default(environment); + + const auto generateStateCoordinates = [&propagator](const State& aState, const Instant& anInstant) -> VectorXd + { + return propagator.calculateStateAt(aState, anInstant).accessCoordinates(); + }; + + const Real mu = + EarthGravitationalModel::EGM2008.gravitationalParameter_.in(Derived::Unit::MeterCubedPerSecondSquared()); + + const Vector3d positionCoordinates = state.getPosition().getCoordinates(); + const Real r = positionCoordinates.norm(); + const Real r3 = std::pow(r, 3); + const Real r5 = std::pow(r, 5); + + const Real x = positionCoordinates(0); + const Real y = positionCoordinates(1); + const Real z = positionCoordinates(2); + + MatrixXd expectedJacobian(6, 6); + expectedJacobian << 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, // first row + 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, // second row + 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, // third row + (-mu / r3) + (3.0 * mu * x * x) / r5, 3.0 * mu * x * y / r5, 3.0 * mu * x * z / r5, 0.0, 0.0, + 0.0, // fourth row + 3.0 * mu * x * y / r5, (-mu / r3) + (3.0 * mu * y * y / r5), 3.0 * mu * y * z / r5, 0.0, 0.0, + 0.0, // fifth row + 3.0 * mu * x * z / r5, 3.0 * mu * y * z / r5, (-mu / r3) + (3.0 * mu * z * z / r5), 0.0, 0.0, + 0.0; // sixth row + + { + const FiniteDifferenceSolver solver = { + FiniteDifferenceSolver::Type::Central, + defaultStepPercentage_, + defaultStepDuration_, + }; + const MatrixXd jacobian = solver.computeJacobian(state, generateStateCoordinates); + + EXPECT_TRUE(jacobian.isApprox(expectedJacobian, 1e-3)); + } + + { + const FiniteDifferenceSolver solver = { + FiniteDifferenceSolver::Type::Forward, + defaultStepPercentage_, + defaultStepDuration_, + }; + const MatrixXd jacobian = solver.computeJacobian(state, generateStateCoordinates); + + EXPECT_TRUE(jacobian.isApprox(expectedJacobian, 1e-3)); + } + + { + const FiniteDifferenceSolver solver = { + FiniteDifferenceSolver::Type::Backward, + defaultStepPercentage_, + defaultStepDuration_, + }; + const MatrixXd jacobian = solver.computeJacobian(state, generateStateCoordinates); + + EXPECT_TRUE(jacobian.isApprox(expectedJacobian, 1e-3)); + } +}