forwardpass#

Forward pass (line search) for iLQR trajectory optimization.

namespace saltro
namespace optimizer

Functions

bool forwardPass(const Satellite &satellite, Eigen::Ref<Eigen::MatrixXd> X, Eigen::Ref<Eigen::MatrixXd> U, const std::vector<Eigen::MatrixXd> &K, const std::vector<Eigen::VectorXd> &d, const Eigen::Ref<const Eigen::Vector2d> &deltaV, const Eigen::Ref<const Eigen::MatrixXd> &B, const Eigen::Ref<const Eigen::MatrixXd> &R, const Eigen::Ref<const Eigen::MatrixXd> &V, const Eigen::Ref<const Eigen::MatrixXd> &S, const Eigen::Ref<const Eigen::MatrixXd> &rho, const Eigen::Ref<const Eigen::MatrixXd> &boresight, const Eigen::Ref<const Eigen::MatrixXd> &attitude_target, const PlannerSettings &settings, const std::vector<Eigen::VectorXd> &lambda_aug, const std::vector<Eigen::VectorXd> &mu_aug, const Eigen::Ref<const Eigen::VectorXd> &jtime, double J_prev, double &J_new)#

Perform forward pass with line search in iLQR.

Executes a backtracking line search to find an improved trajectory using the feedback gains K and feedforward terms d computed by the backward pass. The improved control policy is:

\[ \mathbf{u}_k^{\text{new}} = \mathbf{u}_k + \alpha \mathbf{d}_k + \mathbf{K}_k (\mathbf{x}_k^{\text{new}} - \mathbf{x}_k) \]
where \(\alpha \in (0, 1]\) is the step size found by line search.

The state is propagated forward using the satellite dynamics under the new control. Line search terminates when the Armijo sufficient decrease condition is satisfied:

\[ J^{\text{new}} \leq J^{\text{prev}} + \beta_1 \alpha \nabla J^T \mathbf{d} \]
where \(\beta_1\) is the line search parameter from settings, and the gradient term is approximated using the predicted cost change coefficients deltaV.

If sufficient decrease is not achieved after max_iters backtracking steps, the line search terminates with the best-found trajectory (which may not improve cost).

Parameters:

  • satellite – Satellite model with dynamics, cost, and constraint functions

  • X – State trajectory (N × state_dim); updated in-place with new rollout

  • U – Control trajectory (N-1 × control_dim); updated in-place with new controls

  • K – Feedback gain matrices from backward pass (N-1 vector of nu × nxr matrices)

  • d – Feedforward control terms from backward pass (N-1 vector of nu × 1 vectors)

  • deltaV – Predicted cost change coefficients [Δ_1, Δ_2] from backward pass

  • B – Magnetic field trajectory in ECI frame (3 × N), Tesla

  • R – Position trajectory in ECI frame (3 × N), meters

  • V – Velocity trajectory in ECI frame (3 × N), m/s

  • S – Sun direction trajectory in ECI frame (3 × N), unit vectors

  • rho – Atmospheric density trajectory (1 × N), kg/m³

  • boresight – Desired boresight direction trajectory (3 × N), unit vectors

  • attitude_target – Desired attitude trajectory (4 × N), unit quaternions

  • settings – Planner settings containing line search configuration (beta1, beta2, max_iters)

  • jtime – Mission time vector in Julian centuries (N × 1)

  • J_prev – Total cost of previous trajectory

  • J_new – Output: total cost of new trajectory after line search

Returns:

true if line search succeeded and found improvement, false otherwise

inline bool forwardPass(const Satellite &satellite, Eigen::Ref<Eigen::MatrixXd> X, Eigen::Ref<Eigen::MatrixXd> U, const std::vector<Eigen::MatrixXd> &K, const std::vector<Eigen::VectorXd> &d, const Eigen::Ref<const Eigen::Vector2d> &deltaV, const Eigen::Ref<const Eigen::MatrixXd> &B, const Eigen::Ref<const Eigen::MatrixXd> &R, const Eigen::Ref<const Eigen::MatrixXd> &V, const Eigen::Ref<const Eigen::MatrixXd> &S, const Eigen::Ref<const Eigen::MatrixXd> &rho, const Eigen::Ref<const Eigen::MatrixXd> &boresight, const Eigen::Ref<const Eigen::MatrixXd> &attitude_target, const PlannerSettings &settings, const Eigen::Ref<const Eigen::VectorXd> &jtime, double J_prev, double &J_new)#