Context:
I am studying systems of the form $\dot{x} = M(x)u$ for some state dependent matrix $M(x)\in\mathbb{R}^{n\times m}$ with $m<n$, $x\in\mathbb{R}^n, u\in\mathbb{R}^m$ and initial condition $x(0)=x_0$. My goal is to find $u(t)$ such that the following is minimized: $$ \frac{1}{2}\left(q_fx(T_f)^Tx(T_f) + \int_0^{T_f} qx^Tx+ru^Tu\ dt\right) $$ for some $q,r,q_f,T_f>0$.
I thought of using Pontryagin principle for this problem, so I constructed a Hamiltonian: $$ H = \lambda^TM(x)u + \frac{1}{2}\left(qx^Tx+ru^Tu\right) $$ and obtaining $$ \begin{aligned} \dot{\lambda}&=-\nabla_x H= -\lambda^T\nabla_xM(x)u-qx, &\lambda(T_f)&=q_fx(T_f)^Tx(T_f)/2\\ \dot{x} &= +\nabla_\lambda H = M(x)u,& x(0) &= x_0 \end{aligned} $$
At first glance, I didn't succeed on finding any other simplification of this, so I immediately looked for a numerical alternative. So I attempted the successive approximation method. I proposed an arbitrary $u_0(t)$ and solved (numerically) for $x(t)$ for such input in $t\in[0,T_f]$. Then, once I had $u_0(t), x(t)$ I solved for $\lambda$ by solving (numerically) $$ \dot{\lambda}=+\lambda^T\nabla_xM(x)u+qx, \ \ \ \lambda(0)=q_fx(T_f)^Tx(T_f)/2 $$ for $t\in[0,T_f]$ and then reversing the resulting solution. Having $\lambda(t), x(t)$ I solved for $u_1(t)$ such that: $$ u_1 = \text{argmin}_{u} \lambda^TM(x)u+\frac{q}{2}x^Tx + \frac{r}{2}u^Tu $$ resulting in $$ u_1(t) = -\frac{1}{r}M(x(t))^T\lambda(t) $$ I used this $u_1(t)$ and repeated the procedure to obtain a new $u_2(t)$, similarly to what I did with $u_0(t)$. Ideally, if this procedure converges, it will converge to the optimal $u^*(t)$. However, in my case, I cannot get the method to converge at all: it quickly diverges after a couple iterations.
I tried using $u_0(t) = -M(x)^+ x$, where $M(x)^+$ is the pseudo-inverse, which is a reasonable initial guess for the input. Note that I'm interested in the general $M(x)$ case, but for the examples I'm using $n=3, m=2$ and : $$ M(x) = \begin{bmatrix} 1 & 2+x_1^2 \\ x_2^3+0.1 & 5 \\ -3 & 8+x_1x_3^2 \end{bmatrix} $$
Question:
Do you have any suggestions in order to find the optimal input $u(t)$? Any suggestions or comments would be very helpful.
Motivation: I have an underactuated robotic system, modeled by the aforementioned first-order dynamics, though the matrix $M(x)$ is much much bigger and more complicated. Hence, I am trying simpler (and smaller) expressions for it to start. I intend to solve a problem like this in an MPC setting, every time in a moving horizon fashion.
In general it is difficult to find closed-form solutions for optimal controls. Those explicit solutions only exist in simple cases or when you are lucky.
The difficulty with the Pontryagin's Maximum Principle (PMP) is that an explicit solution relies on the possibility of solving explicitly differential equations. This is almost never possible.
Similarly, when using Dynamic Programming (DP), the difficulty is to find the solution of a (nonlinear) partial differential equation, which is in general a difficult problem. This is the reason why we often relax the problem and consider a partial-differential inequality in order to have more leverage on the construction of the (approximate) value function. Of course, the solution will be suboptimal but we do not have much choice in general. It is either a suboptimal solution or no solution at all!
In order to solve more complex problems numerically, I would recommend to use pseudospectral optimal control methods for which many tools are freely available. See the great book by Roos, "A Primer on Pontryagin's Principle in Optimal Control". You may also check Model Predictive Control, which is often considered as an alternative to optimal control when the PMP or DP are difficult to apply.