feat!: add correct computeJacobian method

bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/Solver/FiniteDifferenceSolver.cpp

@@ -96,49 +96,53 @@ inline void OpenSpaceToolkitAstrodynamicsPy_Solver_FiniteDifferenceSolver(pybind
         )
 
         .def(
-            "compute_jacobian",
+            "compute_state_transition_matrix",
             +[](const ostk::astrodynamics::solver::FiniteDifferenceSolver& solver,
                 const State& aState,
                 const Array<Instant>& anInstantArray,
                 const std::function<MatrixXd(const State&, const Array<Instant>&)>& 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"),
             arg("generate_states_coordinates"),
             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<VectorXd(const State&, const Instant&)>& generateStateCoordinates,
                 const Size& aCoordinatesDimension) -> MatrixXd
             {
-                return solver.computeJacobian(aState, anInstant, generateStateCoordinates, aCoordinatesDimension);
+                return solver.computeStateTransitionMatrix(
+                    aState, anInstant, generateStateCoordinates, aCoordinatesDimension
+                );
             },
             R"doc(
                 Compute the jacobian.
 
                 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<VectorXd(const State&, const Instant&)> 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.

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)

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,24 +92,24 @@ 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<Instant>& anInstantArray,
         const std::function<MatrixXd(const State&, const Array<Instant>&)>& 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.
     /// @param generateStateCoordinates Callable to generate coordinates of a State at the
     /// 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<VectorXd(const State&, const Instant&)>& generateStateCoordinates,
@@ -127,6 +127,16 @@ class FiniteDifferenceSolver
         const State& aState, const std::function<VectorXd(const State&, const Instant&)>& 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<VectorXd(const State&, const Instant&)>& generateStateDerivatives
+    ) const;
+
     /// @brief Print the solver.
     ///
     /// @param anOutputStream An output stream.

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<Vector5d>(
         finiteDifferenceSolver_
-            .computeJacobian(stateBuilder_.build(Instant::J2000(), aCOEVector), Instant::J2000(), getQ, 1)
+            .computeStateTransitionMatrix(stateBuilder_.build(Instant::J2000(), aCOEVector), Instant::J2000(), getQ, 1)
             .data(),
         5
     );

src/OpenSpaceToolkit/Astrodynamics/Solvers/FiniteDifferenceSolver.cpp

@@ -50,7 +50,7 @@
     return stepDuration_;
 }
 
-MatrixXd FiniteDifferenceSolver::computeJacobian(
+MatrixXd FiniteDifferenceSolver::computeStateTransitionMatrix(
     const State& aState,
     const Array<Instant>& anInstantArray,
     const std::function<MatrixXd(const State&, const Array<Instant>&)>& generateStateCoordinates,
@@ -138,7 +138,7 @@
                                 ? aState.accessCoordinates()(i) * stepPercentage_
                                 : stepPercentage_;
 
-        MatrixXd differencedCoordinates = computeBlock(stepSize, aState.getCoordinates(), i);
+        MatrixXd differencedCoordinates = computeBlock(stepSize, aState.accessCoordinates(), i);
 
         const VectorXd columnStackedCoordinates =
             Eigen::Map<VectorXd>(differencedCoordinates.data(), differencedCoordinates.size());
@@ -149,7 +149,7 @@
     return A;
 }
 
-MatrixXd FiniteDifferenceSolver::computeJacobian(
+MatrixXd FiniteDifferenceSolver::computeStateTransitionMatrix(
     const State& aState,
     const Instant& anInstant,
     const std::function<VectorXd(const State&, const Instant&)>& generateStateCoordinates,
@@ -167,7 +167,7 @@
         return coordinatesMatrix;
     };
 
-    return computeJacobian(aState, {anInstant}, generateStatesCoordinates, aCoordinatesDimension);
+    return computeStateTransitionMatrix(aState, {anInstant}, generateStatesCoordinates, aCoordinatesDimension);
 }
 
 VectorXd FiniteDifferenceSolver::computeGradient(
@@ -178,7 +178,7 @@
     {
         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 @@
 
         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 @@
     }
 }
 
+MatrixXd FiniteDifferenceSolver::computeJacobian(
+    const State& aState, const std::function<VectorXd(const State&, const Instant&)>& 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<VectorXd(const Real&, const Index&)> 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)