# How to numerically solve an optimal thrust-vectoring problem?

This question is a follow-up to this question and is linked to this question, where if needed more details can be found on the parameters used in the following.

For an in-plane non-impulsive orbital maneuver, I'd like to find the thrust-direction history $\beta(t)$ to minimize the time needed to transfer the spacecraft from a specified initial state to a specified terminal state, subject to the following state equations:

$$\dot{\textbf{x}}(\textbf{x},\beta,t)=\begin{bmatrix}\dot{r} & \dot{V}_r & \dot{V}_{\theta}\end{bmatrix}$$ $$\dot{r}=v_r$$ $$\dot{v}_r=\frac{v_{\theta}^2}{r}-\frac{\mu}{r^2}+\frac{T\sin{\beta}}{m_0-|\dot{m}|t}$$ $$\dot{v}_{\theta}=-\frac{v_{r}v_{\theta}}{r}+\frac{T\cos{\beta}}{m_0-|\dot{m}|t}$$

where $\mu$, $T$, $m_0$ and $\dot{m}$ are constants, with state variables:

$$\textbf{x}(t)=\begin{bmatrix}r & V_r & V_{\theta}\end{bmatrix}$$

and initial and terminal conditions:

\begin{align*} \textbf{x}(t_0) &=\begin{bmatrix}1745100 & 0 & 2027\end{bmatrix}\\ \textbf{x}(t_f) &=\begin{bmatrix}1765100 & 0 & 0\end{bmatrix}\\ \end{align*}

i.e. we deal with a two-point boundary value problem where a spacecraft transfers from an initial position to a higher position, while decreasing its initial velocity to zero terminal velocity. The problem is cast as an optimal control problem for which I have formulated the following minimum-time cost functional:

$$J=\min_{\beta}\int_{t_0}^{t_f}L(\textbf{x},\beta,t)dt=\min_{\beta}\int_{t_0}^{t_f}(1)dt$$

where $L$ denotes the Lagrangian. The Hamiltonian is, therefore,

\begin{align*} H(\textbf{x},\beta,t) &= L(\textbf{x},\beta,t)+\lambda^T\dot{\textbf{x}}(\textbf{x},\beta,t) \\ &= 1+\lambda_{r}\dot{r}+\lambda_{v_r}\dot{v}_r+\lambda_{v_{\theta}}\dot{v_{\theta}} \\ &= 1+\lambda_{r}\cdot v_r+\lambda_{v_r}\cdot (\frac{v_{\theta}^2}{r}-\frac{\mu}{r^2}+\frac{T\sin{\beta}}{m_0-|\dot{m}|t})+\lambda_{v_{\theta}}\cdot (-\frac{v_{r}v_{\theta}}{r}+\frac{T\cos{\beta}}{m_0-|\dot{m}|t}) \end{align*}

where $\mathbf{\lambda}(t)$ is the Lagrange multiplier vector, whose elements are the costate variables. The $\lambda$-dynamics that capture the behavior of the Lagrange multipliers, also known as the costate equations, are given by:

\begin{align*} \dot{\lambda}_{r} =-\frac{\delta H}{\delta r} &= -\lambda_{v_r} \cdot (-\frac{v_{\theta}^2}{r^2}+\frac{2\mu}{r^3}) - \lambda_{v_{\theta}} \cdot (\frac{v_{r}v_{\theta}}{r^2})\\ \dot{\lambda}_{v_r} =-\frac{\delta H}{\delta v_r} &= -\lambda_{r} + \lambda_{v_{\theta}} \cdot (\frac{v_{\theta}}{r})\\ \dot{\lambda}_{v_{\theta}} =-\frac{\delta H}{\delta v_{\theta}} &= -\lambda_{v_r} \cdot (\frac{2v_{\theta}}{r}) + \lambda_{v_{\theta}} \cdot (\frac{v_r}{r})\\ \end{align*}

The partial of $H$ with respect to $\beta$ must equal zero, so

$$\frac{\delta H}{\delta \beta}=\lambda_{v_r} \cdot (\frac{T\cos{\beta}}{m_0-|\dot{m}|t}) - \lambda_{v_{\theta}} \cdot (\frac{T\sin{\beta}}{m_0-|\dot{m}|t})=0$$

leading to the following control law:

$$\tan{\beta}=\frac{-\lambda_{v_r}}{-\lambda_{v_{\theta}}}$$

or,

$$\sin{\beta}=\frac{-\lambda_{v_r}}{\sqrt{\lambda_{v_r}^2+\lambda_{v_{\theta}}^2}} \qquad \cos{\beta}=\frac{-\lambda_{v_{\theta}}}{\sqrt{\lambda_{v_r}^2+\lambda_{v_{\theta}}^2}}$$

Note: the signs in the ratio are negative because we are looking for a minimum.

This is the first time I'm working with an optimal control problem, and I have some trouble understanding how to proceed from here and solve the problem with MATLAB. Is there perhaps someone out here who knows how to do this and could explain what the next steps would be?

If you want to keep your formulation and optimize the problem as a TBVP, I would suggest you to use a shooting method. Since you have a first order system with six differential equations (and you have the relation between the control variable and adjoint variables), you can integrate your dynamic system with a known initial condition with ode45 or similar. As you will notice the problem is that you do not know the initial values for your adjoint variables and time, so you have to made a guess on them. The idea is to match your terminal conditions, $\mathbf{x}(t_f)$ and the transversality hamiltonian condition due to final time (which you do not have take into account), in your case is $H(t_f)=0$. This could be done with a Newton's method using fsolve MATLAB function. Resuming the process:

1. Make a guess on your unknown initial conditions, $\pmb{\lambda}(t_0)$ and $t_f$.

2. With those parameters simulate your problem with an integrator as for example, ode45.

3. Use a Newton method with a sensitivity matrix to change your guess and rerun step 2 with a better guess to match the desired $\mathbf{x}(t_f)$ and $H(t_f)=0$. This could be done easily using fsolve MATLAB function embedding the ode45 integration into it.
4. The output of fsolve function is a candidate to the optimal control problem.

Now, I will tell you some advices, considering your problem I would consider to integrate backwards because you only have to meet the initial state since you can guarantee $H(t_f)=0$ with your initial guess. Also, it is not guaranteed that the problem is extremely sensitive to initial conditions (this has happened to me in some cases) and a more brute force method to solve the TPBVP have to be used like bvp4c or bvp5c MATLAB functions (but, I would recommend that only as the last option).

Another advice is that I would consider to solve the problem with direct methods rather than an indirect one because as you will notice obtaining a good initial guess of the adjoint variables is not a trivial issue.

• Nice answer! I'm curious if integrating backwards has any advantage. Going forward in time, one solves for a given altitude and velocity (zero), but going backwards one also needs to solve for a certain altitude and velocity as well (circular orbit). Am I missing something, or are they actually pretty much equivalent? – uhoh Oct 30 '17 at 11:42
• Yes, it has an advantage. You are missing the transversality condition $H(t_f)=0$ (the OP has missed it too) since final time is unspecified. So, integrating forward you have $[\pmb{\lambda}(t_0), t_f]$ to match $[\mathbf{x}(t_f), H(t_f)=0]$, you have to guess 4 parameters to match another 4 parameters. But, if you integrate backwards since you have to guarantee $H(t_f)=0$ a relation between $\lambda(t_f)$ is available and in this case you just need to guess two adjoint variables thus reducing by one the number of guesses. Now you match $\mathbf{x}_0$ with two $\lambda(t_f)$ and $t_f$ – Julio Oct 30 '17 at 14:01
• Thanks for your suggestions. Indeed, finding proper initial values for the adjoint variables is not straightforward at all. I found a trajectory optimization library for MATLAB though, called OptimTraj, which allowed me to solve the problem above with direct collocation, using both the trapezoid method and Hermite-Simpson method. – woeterb Oct 30 '17 at 16:15
• @JulioCesarSanchezMerino OK thanks, I'll take a look. – uhoh Oct 30 '17 at 18:19