\begin{eqnarray*} {\bf x}_{k+1} &=& {\bf A}{\bf x}_k + {\bf B} u_{k} \\ p_k &=& {\bf C} {\bf x}_{k} \\ \end{eqnarray*}
For this we need to declare the associated matrices:
and the weights of the function to be minimized:Then we have to initialize the discretized linear system with
\begin{eqnarray*} {\bf A} & \equiv & \left[ \begin{matrix} 1 & T & T^2/2 \\ 0 & 1 & T \\ 0 & 0 & 1 \end{matrix} \right] \\ {\bf B} & \equiv & \left[ \begin{matrix} T^3/6 \\ T^2/2 \\ T \end{matrix} \right] \\ {\bf C} & \equiv & \left[ 1 \; 0 \; \frac{-z_c}{g} \right] \end{eqnarray*}
We then have to initialize the weights of the index function:
\[ J = \sum^{NL}_{j=1} \{ Q(p^{ref}_j -p_j)^2 + Ru_j^2 \} \]
To suppress the problem of the initial CoM position, we can reformulate the discrete problem by posing the following:
\begin{eqnarray*} {\bf x}^*_{k+1} = \widetilde{\bf A} {\bf x}^*_{k} + \widetilde{\bf b}\Delta u_k p_k = \widetilde{\bf c}{\bf x}^*_{k} \end{eqnarray*}
with {eqnarray* u_k u_k - u_{k-1} & { x}_k { x}_k - { x}_{k-1} { x}_k [ {matrix} p_k\ { x}_k {matrix} ] }
\begin{eqnarray*} \widetilde{\bf A} &\equiv & \left[ \begin{matrix} 1 & {\bf cA} \\ {\bf 0} & {\bf A} \\ \end{matrix} \right] \\ \tilde{\bf b} & \equiv & \left[ \begin{matrix} {\bf cb} \\ {\bf c} \end{matrix} \right] \\ \tilde{\bf c} & \equiv & [ 1 \; 0 \; 0 \; 0] \\ \end{eqnarray*}
Then the subsequent code performs this operation and displays the associated matrices:
To create the instance of the object solving the Riccati Equation:
The computation of the weights is done by calling ComputeWeights(). There is only one parameter to specify, but it is important as the weights are computed differently according to this parameter. If you use the mode without initial position please uses MODE_WITH_INITIALPOS.
To display the weights in the standard output
It is possible to retrieve the weights in a vector: