I am interested in solving a optimal control input problem with discrete finite time when the reference state value is given, I have designed a cost function with terminal state and then I want use the following equations to compute the optimal $u$

\begin{equation} \begin{aligned} \label{Kostenfunktion} J_{1}(k)&=\min_{\mathbf{u}}\sum_{j=1}^{N_p-1} \|( \mathbf{x}(k+j|k) -\mathbf{R_{ref}}(k+j) )\|_{\mathbf{Q_i}}^2 + \sum_{j=0}^{N_c-1}\|\mathbf{u}(k+j)\|_{\mathbf{P_i}}^2+ \|\mathbf{x}(k+Np|k)-x_F\|_W^2 \\ \mathbf{H}&=\mathbf{M}_{u}^{T}\mathbf{Q}\mathbf{M}_{u}+\mathbf{P}\\ \mathbf{f}^T&=-\mathbf{M}_{u}^{T}\mathbf{Q}(\mathbf{R}_{ref}-\mathbf{M}_{x}x_{k})\\ \mathbf{u}&=-inv(\mathbf{H})*\mathbf{f}\\ \end{aligned} \end{equation}

$\mathbf{M}_{x}=\begin{bmatrix} A\\ A^2\\ A^3\\ \vdots \\ A^{N_p} \end{bmatrix}$ , $\mathbf{M}_{u} = \begin{bmatrix} B &0 &0 &\cdots & 0\\ AB & B & 0 & \cdots & 0\\ A^2B& AB & 0 &\cdots &0 \\ \vdots & \vdots & \vdots & \vdots &\vdots \\ A^{N_p-1}B & A^{N_p-2}B & A^{N_p-3}B & \cdots & A^{N_p-N_c}B \end{bmatrix}$

I have two questions

  1. How can I choose the terminal state? could I choose free value? (I know there would be some convergence problems when choose free)
  2. I would like to using this cost function to compute a trajectory with distance, speed and acceleration. the dynamic model is \begin{equation} x(k+1) = Ax(k) + Bu(k) \\ \end{equation}

A=[0 1 0; 0 0 1;0 0 0];
B=[0 0 1];

I would like to compute a trajectory within, its initial condition is [0 0 0] and the final condition is [10,2.7,0] ($R_{ref}$ is also the final condition), $N_p=N_c=50$, but from the following figure we can see that the end position couldn't reach the defined end point, I would like to ask, it's about the tuning parameters or the cost function has some problem? Thanks

enter image description here

  • $\begingroup$ You should expand the abbreviation "MCP" . It might help readers if you could characterize the nature of your question: "I have a conceptual problem with the general theory of XYZ" or "I have a calculational problem in applying XYZ" or "I want a sanity check on my proposed thesis topic", or whatever. $\endgroup$ Nov 5, 2019 at 19:45
  • $\begingroup$ What values for $N_p$ and $N_c$ are you using and did you only solved for the optimal input once and plotted its predicted trajectory or did you only apply the first input of each optimal solution and increased $k$ each time? $\endgroup$ Nov 5, 2019 at 22:48
  • $\begingroup$ @KwinvanderVeen thank you for your answer. I choose Np=Nc=50, and as you said, I just apply the first input of each optimal solution and increased k at each time. $\endgroup$
    – Yunhua Hu
    Nov 6, 2019 at 8:12
  • $\begingroup$ @kimchilover thanks $\endgroup$
    – Yunhua Hu
    Nov 6, 2019 at 8:15

1 Answer 1


In order to have that the final state is equal to a desired state one could remove the terminal cost term and replace it with an equality constraint. When solving this optimization problem you could use a solver which allows for this, or if you want to do it manually use Lagrange multipliers. Though the equality constraint can also be approximated by making $W$ really big.

It can be noted that the solution only guarantees that $x(k+N_p|k)$ is equal to the desired state. However, if you solve the problem each time $k$ is increased and you only apply the first input of the solution, then you only reach $x(k+1|k)$, $x(k+2|k+1)$, etc. but never $x(k+N_p|k)$. To fix this one could reduce $N_p$ and $N_c$ by one every time $k$ increases, but this should just give the same solution as the fist solution (if the model matches perfectly to the actual system). It can be noted that it is possible to asymptotically converge to the desired state using your implementation, if there exists an input $u^*$ such that the desired state $x^*$ is stationary, so satisfies $x^*=A\,x^* + B\,u^*$ and the cost function is defined as

$$ J(k) = \min_u \sum_{j=1}^{N_p-1} \|x(k+j|k) - x^*\|_{Q_i}^2 + \sum_{j=0}^{N_c-1} \|u(k+j) - u^*\|_{P_i}^2. $$

The equality constraint isn't necessary to get the asymptotic convergence of the state to $x^*$.

PS: Your model $(A,B)$ is a chain of delays, while your labels of your figures suggests that you intended it to be a chain of integrators (your model would be a chain of integrators if it where continuous time, $\dot{x} = A\,x + B\,u$). A chain of integrators in discrete time could for example be obtained through zero order hold using

$$ A = \begin{bmatrix} 1 & h & 1/2\,h^2 \\ 0 & 1 & h \\ 0 & 0 & 1 \end{bmatrix}, \quad B = \begin{bmatrix} 1/6\,h^3 \\ 1/2\,h^2 \\ h \end{bmatrix}, $$

with $h$ the sample time.

  • $\begingroup$ Thank you so much for your explanation, that's really helpful! $\endgroup$
    – Yunhua Hu
    Nov 6, 2019 at 12:28
  • $\begingroup$ I have a little question, could I use first order hold(FOH) to discrete the state space model? may be more accurate? $\endgroup$
    – Yunhua Hu
    Nov 6, 2019 at 14:11
  • $\begingroup$ @YunhuaHu I don't see why not. $\endgroup$ Nov 6, 2019 at 14:58
  • $\begingroup$ I have a little question. If I use first oder hold to discrete the state space modul, only B is different from the ZOH method. If we use the same sampling time, what's the difference between the ZOH and FOH discrete method? $\endgroup$
    – Yunhua Hu
    Jan 3, 2020 at 12:34
  • $\begingroup$ ZOH assumes that the input it held constant during one sample time, while FOH assumes that the input is linearly interpolated between sample times. So these discretization methods only affects the assumed behavior of the input signal. The dynamics of the system, given certain initial conditions, is solved exactly, which is the same in both cases and thus the discretized A is the same. $\endgroup$ Jan 3, 2020 at 17:00

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.