Using fmincon to solve objective function in integral form.

Hello, I'm attempting to solve an optimal control problem using the fmincon function however I have very little experience with it. I'm wondering if there is some fundamental flaw to my code or if there is a more obscure mistake like using the trapz function for integration. My current batch of code is no longer spitting out errors so trying to troubleshoot my way to a solution has slowed down.

The problem is as follows:

And here is the code:

%% Optimization
clear all
close all
clc

%Initialize constants
global U tspan N div 
tspan = 10;
div = 10;
N = tspan*div;

control_ic = [linspace(0, 10, 1/5*N), linspace(10, 0, 4/5*N)]; %Control profile guess
%% Routine
u_t = fmincon(@fun, control_ic, [], [],[],[], [], [], [], optimoptions('fmincon', 'MaxFunctionEvaluations', 1e5))

Local minimum possible. Constraints satisfied.

fmincon stopped because the size of the current step is less than
the value of the step size tolerance and constraints are 
satisfied to within the value of the constraint tolerance.

u_t = 1x100

-4.7060 17.2991 2.8918 -25.1503 23.6414 5.3004 -0.3443 -1.4582 8.0192 13.3754 5.3292 -2.3025 11.8676 6.8500 9.7229 6.1461 6.4091 8.8996 12.3205 10.0425 3.6941 14.0695 2.2968 11.0991 9.4470 13.5367 -0.5043 12.9030 5.6669 8.4720

<mw-icon class=""></mw-icon>

<mw-icon class=""></mw-icon>

function [J] = fun(u) %Objective function
global U tspan div 
U = u;
dynamic_ic = [2;2]; %Initial Conditions

[t, X] = ode45(@dynamics, [0 tspan], dynamic_ic); %Find x, xd using u
x = X(:, 1);
xd = X(:, 2);

x_cost = trapz(tspan/length(t), 2*x.^2 + 2*xd.^2); %Calculate objective function
finalx_cost = x(length(x))^2;
control_cost = trapz(div^(-1), .1*u.^2);

J = x_cost + finalx_cost + control_cost;
end

%System Dynamics
function [X] = dynamics(t, x)
global U tspan N 
xd = x(2);  
vd = -.2*x(2) - 3*x(1) + interp1(linspace(0, tspan, N), U, t);
X = [xd; vd];
end

[Dyuman Joshi]

Programmatically, I would suggest you to remove the 1st three lines of code, and, remove the global variables and provide them as inputs to the functions and call accordingly.

https://in.mathworks.com/help/matlab/matlab_prog/techniques-for-improving-performance.html

[Bruno Luong]

Also on the control point of view you should regularize the control u by adding a penaty on 2-norm of du/dt in the cost function. This avoid oscillation.

Answer 1

[Bruno Luong]

trapz(tspan/length(t), 2*x.^2 + 2*xd.^2)

This looks wrong to me, you seem to assume t is equi distance (even in that case you should duvide by length(t)-1).

I woild say this formula

trapz(t, 2*x.^2 + 2*xd.^2)

returns what you want. Warning I do not test, and possibly there is still other mistake in your code.

[Bruno Luong]

Code with faulty trapz fixed and add smoothing term.

I also remove the global variables and fix a small issue with N that is 1 shorter than the right value.

%% Optimization
%Initialize constants
tspan = 10;
div = 10;
N = round(tspan*div+1);

tu = linspace(0, tspan, N);
control_ic = interp1(tspan*[0 1/5 1],[0 10 0], tu);
%% Routine
u_t = fmincon(@(u) fun(tu,u), control_ic, [], [],[],[], [], [], []);

Local minimum possible. Constraints satisfied.

fmincon stopped because the size of the current step is less than
the value of the step size tolerance and constraints are 
satisfied to within the value of the constraint tolerance.

figure
plot(tu, u_t);
xlabel('t');
ylabel('u');

function [J] = fun(tu, u) %Objective function
dynamic_ic = [2;2]; %Initial Conditions
tspan = tu(end);
[t, X] = ode45(@(t,x) dynamics(t,x,tu,u), [0 tspan], dynamic_ic); %Find x, xd using u
x = X(:, 1);
xd = X(:, 2);

x_cost = trapz(t, 2*x.^2 + 2*xd.^2); %Calculate objective function
finalx_cost = x(end)^2;
dt = tu(2)-tu(1);
control_cost = trapz(dt, .1*u.^2);
control_smoothbess_cost = trapz(dt, 1*gradient(u,dt).^2); % Add by BLU to smooth the control

J = x_cost + finalx_cost + control_cost + control_smoothbess_cost;
end

%System Dynamics
function [X] = dynamics(t, x, tu, u)
xd = x(2);  
vd = -.2*x(2) - 3*x(1) + interp1(tu, u, t);
X = [xd; vd];
end

[Sam Chak]

Hi @Joseph Criniti

I have compared your fmincon optimal control result with the one generated by the standard LQR optimal control algorithm. Both methods employed a similar quadratic objective function, although your objective function in fmincon included additional terms. However, I believe that the main issue may stem from your choice of the "Control profile guess." Could you please explain the rationale behind selecting the triangular-shaped profile?

More importantly, does the settings in fmincon satisfy the conditions of Pontryagin's principle of optimality?

%% Initialize constants
tspan   = 10;
div     = 20;
N       = round(tspan*div+1);

tu      = linspace(0, tspan, N);

%% Control profile guess
control_ic = interp1(tspan*[0 1/5 1], [0 10 0], tu);
figure(1)
plot(tu, control_ic), grid on, xlabel('t'), ylabel('u(t)'), title('Control profile guess')

%% Joseph's Optimal Control Routine 
u_t     = fmincon(@(u) fun(tu, u), control_ic, [], [], [], [], [], [], []);

Local minimum possible. Constraints satisfied.

fmincon stopped because the size of the current step is less than
the value of the step size tolerance and constraints are 
satisfied to within the value of the constraint tolerance.

[t1, x1]= ode45(@(t, x) dynamics(t, x, tu, u_t), [0 10], [2; 2]);

%% ~ LQR Optimal Control ~
A       = [0,  1;           % state matrix
          -3, -0.2];
B       = [0;               % input matrix
           1];
Q       = diag([2, 2]);     % state-cost weighted matrix
R       = 0.1;              % input-cost weighted matrix
K       = lqr(A, B, Q, R);  % Optimal control gain
tsim    = [0 10];
x0      = [2; 2];
[t2, x2]= ode45(@(t, x) odefcn(t, x, A, B, K), tsim, x0);
[~,uLQR]= odefcn(t2', x2', A, B, K);

%% Plot results
figure(2)
tL1     = tiledlayout(2, 1, 'TileSpacing', 'Compact');
nexttile
plot(t1, x1(:,1)), grid on, ylabel('x(t)'), title('System response')
nexttile
plot(tu, u_t),     grid on, ylabel('u(t)'), title('Control signal')
xlabel(tL1, 'Time')
title(tL1, 'fmincon Optimal Control')

figure(3)
tL2     = tiledlayout(2, 1, 'TileSpacing', 'Compact');
nexttile
plot(t2, x2(:,1)), grid on, ylabel('x(t)'), title('System response')
nexttile
plot(t2, uLQR),    grid on, ylabel('u(t)'), title('Control signal')
xlabel(tL2, 'Time')
title(tL2, 'LQR Optimal Control')

%% Objective function
function [J] = fun(tu, u)
    dynamic_ic   = [2;2]; % Initial Conditions
    tspan        = tu(end);
    [t, X]       = ode45(@(t,x) dynamics(t, x, tu, u), [0 tspan], dynamic_ic); % Find x, xd using u
    x            = X(:, 1);
    xd           = X(:, 2);
    
    x_cost       = trapz(t, 2*x.^2 + 2*xd.^2); % Calculate objective function
    finalx_cost  = x(end)^2;
    dt           = tu(2) - tu(1);
    control_cost = trapz(dt, .1*u.^2);
    control_smoothbess_cost = trapz(dt, 1*gradient(u, dt).^2); % Add by BLU to smooth the control
    
    J            = x_cost + finalx_cost + control_cost + control_smoothbess_cost;
end

%% System Dynamics
function dX = dynamics(t, x, tu, u)
    xd = x(2);  
    vd = -.2*x(2) - 3*x(1) + interp1(tu, u, t);
    dX = [xd; vd];
end

%% State-Space (for LQR controller)
function [dx, u] = odefcn(t, x, A, B, K)
    u   = - K*x;
    dx  = A*x + B*u;
end

[Bruno Luong]

@Sam Chak LQR looks very good.

I was also surprised by the poor performance of fmincon/ode45. After all it is a linear control and it just get stuck around the initial control. Anyway I think this is just a toy example or homework, but quite educative.

[Joseph Criniti]

Firstly, I greatly appeciate everyone that took an ounce of time to look at this problem. The initial guess was formulated from nothing specific other than having breifly seen the actual problem solution (which is basically the LQR solution). That solution was directly achieved through minimizing the Hamiltonian, with a very poor initial guess which I believe was all 1's. Unfourtnatlely finding good resources on this process is difficult so I instead tried to enlist Matlabs help with fmincon.

• I agree with the fix to the trapz function using t as the input argument. Fixed step simulink has made me naive.
• I appreciate the syntax for calling variables into functions used by other functions. I knew this existed but was unsure on the syntax.

ode45(@(t,x) dynamics(t, x, tu, u), [0 tspan], dynamic_ic);

• As to Pontryagin's Principle of Optimality, I have a general knowledge of its significance, but no working knowledge for problem solving. Perhaps, there is more reading in my future.

After implementing all the suggested changes the code still doesnt reach any solution resembling the LQR solution. Again, I greatly appreciate the help but I think implementing something closer to the solver I've previously seen is the next step, though I did build some insight from this experiment so its not a full loss.

Thanks,

Joe

[Sam Chak]

Hi @Joseph Criniti

If you are referring to the Hamiltonian function for your 2nd-order system, then there should be two Costate equations present (also see here). However, they are nowhere to be seen in your objective function. This incomplete formulation of the time-step optimization problem is causing the fmincon solver to become stuck at the initial guess profile, as mentioned by @Bruno Luong.

Nevertheless, it is entirely possible for fmincon to yield the same optimal gains as the LQR algorithm if both methods use the same quadratic cost function.

[Bruno Luong]

I suspect that the ode45 solver by using complicated time step adaptive somewhat defeats fmincon.

If the time step is fixed and simple discretization, the ode system become trully numerical linear, fmincon would not stuck.

[Bruno Luong]

My suspicious is correct, using simple ode solver, fmincon has clearly better convergence

%% Optimization

%Initialize constants
tspan = 10;
div = 16;
N = round(tspan*div+1);

tu = linspace(0, tspan, N);
control_ic = interp1(tspan*[0 1/5 1],[0 10 0], tu);
%% Routine
u_t = fmincon(@(u) fun(tu,u), control_ic, [], [],[],[], [], [], [], ...
    optimoptions('fmincon', 'MaxFunctionEvaluations', 1e5));

Local minimum found that satisfies the constraints.

Optimization completed because the objective function is non-decreasing in 
feasible directions, to within the value of the optimality tolerance,
and constraints are satisfied to within the value of the constraint tolerance.

dynamic_ic = [2;2]; %Initial Conditions
tspan = tu(end);
[tode45, Xode45] = ode45(@(t,x) dynamics(t,x,tu,u_t), [0 tspan], dynamic_ic);
[t, X] = linearode(dynamic_ic, tu, u_t);

figure
subplot(2,1,1)
plot(tu, u_t);
xlabel('t'); ylabel('u'); title('Control');
subplot(2,1,2);
plot(tode45, Xode45(:,1),'g',t, X(:,1),'.b');
legend('ode(5', 'linearode');
xlabel('t'); ylabel('x'); title('Signal response');

function [J] = fun(tu, u) %Objective function
dynamic_ic = [2;2]; %Initial Conditions

%Using ode45 is not suitable for fmincon
%tspan = tu(end);
%[t, X] = ode45(@(t,x) dynamics(t,x,tu,u), [0 tspan], dynamic_ic); %Find x, xd using u
[t, X] = linearode(dynamic_ic, tu, u);
x = X(:, 1);
xd = X(:, 2);
x_cost = trapz(t, 2*x.^2 + 2*xd.^2); %Calculate objective function
finalx_cost = x(end)^2;
dt = tu(2)-tu(1);
control_cost = trapz(dt, .1*u.^2);
control_smoothbess_cost = trapz(dt, 1*gradient(u,dt).^2); % Add by BLU to smooth the control

J = x_cost + finalx_cost + control_cost + control_smoothbess_cost;
end

%System Dynamics
function [X] = dynamics(t, x, tu, u)
xd = x(2);  
vd = -.2*x(2) - 3*x(1) + interp1(tu, u, t,'previous');
X = [xd; vd];
end

% Integration linear differiential equation
function [t, X] = linearode(X0, tu, u)

% ode: dX/dt = A*X(t) + U(t)
% X(t=0) = X0
% Uf we want to optimize most matrices can be computed before hand
dt = tu(2)-tu(1);
t = tu(:);
N = size(t,1);
X = zeros(N,2);
Xn = X0;
X(1,:) = Xn;
A = [ 0,  1;
     -3, -0.2];
E = expm(A*dt);
I = eye(size(A));
G = A \ (inv(E)-I);
for n = 2:N
    ut = u(n-1);
    Ut = [0; ut];
    Xn = E*(Xn - G*Ut);
    X(n,:) = Xn;
end
end

[Sam Chak]

@Bruno Luong, That's a good point. I completely overlooked the adaptive time step feature in the ode45 solver.

@Joseph Criniti, I have provided a fixed time-step RK4 ode solver for you. Feel free to experiment with it and make modifications as needed. The syntax is as follows:

X = oderk4(@(t, x) dynamics(t, x, tu, u), tu, dynamic_ic); 

%% Runge-Kutta 4th-order ODE Solver

function y = oderk4(f, x, y0)

y(:,1) = y0; % initial condition

h = x(2) - x(1); % step size

n = length(x); % number of steps

for j = 1 : n-1

k1 = f(x(j), y(:,j)) ;

k2 = f(x(j) + h/2, y(:,j) + h/2*k1);

k3 = f(x(j) + h/2, y(:,j) + h/2*k2);

k4 = f(x(j) + h, y(:,j) + h*k3) ;

y(:,j+1) = y(:,j) + h/6*(k1 + 2*k2 + 2*k3 + k4);

end

end

[Bruno Luong]

As now it converge I switch off the smoothing term of the control (the 0 coefficient vs 1)

control_smoothbess_cost = trapz(dt, 0*gradient(u,dt).^2); % Add by BLU to smooth the control

The result looks similar to LQR obtained by @Sam Chak

The math looks alright and consistent.

[Sam Chak]

@Bruno Luong +1 😄 👍

[Bruno Luong]

I attach here the script with 3 methods of ODE solvers, selection in line #10.

Feel free to play with it.

[Joseph Criniti]

Again, thanks so much for the help.

@Sam Chak This was not meant to be an implementation of the Hamiltonian. That was how I solved thos problem first solved but since I was clueless on the process I decided to use fmincon.

@Bruno Luong Wow, I have no words. I've implemented the fixed-step runge4 solver and it works perfect. I don't want to eat anymore of your time but why does fmincon stumble when presented with a variable step integration solution? Late last night I was wondering if integration errors were leading to the poor results so I went ahead and wrote a cubic splines integrator.

%Cubic Splines Integrator 
%Joe Criniti 4/11/24
function [I] = cusplint(x, y)

if length(x) == 1                    %x can be a vector with corresponding y 
    x = 0:x:(length(y)*x-x);         %Or x can be fixed divisions between y
end

spl = spline(x, y);                  %Spline coefficients
I = 0;
for i = 1:spl.pieces
    q = polyint(spl.coefs(i, :));                                       %Integrated spline coefficients (4th order)
    dif = spl.breaks(i+1)-spl.breaks(i);                                %Splines are evaluated from 0 -> dif
    I = I + q(1)*dif^4 + q(2)*dif^3 + q(3)*dif^2 + q(4)*dif + q(5);     %Rolling sum of integration through splines                                                                    
end
end

This did little in the way of improving performance but I imagine for a more coarse step size it could outperform trapz. It was only a few times slower than trapz so I left in.

Like I mentioned before, when I saw this problem solved I could have sworn that an initial guess of just ones was used.

control_ic = [0, ones(1, N-1)]; 

I tested this out and it worked fine. Im sure convergence would have happened much sooner if the guess was better but I was very surpirsed to see the solver is that robust. The fixed step runge4 completely eliminated the high sensitivity to the initial condition.

Lastly, I noticed that the interp1 function takes about half the solver runtime. This was very surprising because I didnt think linear interpolation was remotely that expensive. The cubic spines functions operated in about a quarter of the time and It had to handle 99 splines. I made an attempt to systematically index u in the dynamics now that u and x are on the same time step, but it got alittle weird, results similar but It changed the control input slightly so I decided to leave it alone for now.

Once again, I cannot thank you guys enough for your time.

[Bruno Luong]

The issue is not variable step of ode per se. The ossue is that fmincon performs finite difference scheme to estimate the gradient of the cost function. If the time steps change between 2 evaluations of the ode model, the gradient estimated is incirrect, thus fmincon fails to estimate the correct descend direction and fails to converge

Your model is linear, the objective is quadratic wrt to the control, meaning convex. Even strictly convex since you integrate thé square of the control (its 2 norm). It must converge to the right solution regardless the initial strating point of the control.

The starting point only matter if you works with non linear ode model.

Be careful about using spline for integration, the spline functionmight become negative even for positive data. At the end what matter is you should do positive weighted sum of the square of a li,ear map of the state variables. If your numerical schem do not lead to this condition it will create issue at some point.

If you really to optimize the code save time you could use te adjoint equation to compute the gradient. There is a whole field about this theory and it is tricky to implement it correctly. But this is another topic/.

[Bruno Luong]

I extend my linear ode equation solver with the forcing term u(t) - the control - to piecewise linear (it was piecewise constant).

It must be out there someone who wrote formula recipe for linear ode with generic polynomial forcing term. I do it by hand so it is a little bit painful to increase the degree.

EDIT I think have derived a general closed formula to solve linear ode with constant coefficient and general polynomial forcing U(t).

dX/dt = A*X + U(t)

[Joseph Criniti]

Thank you for your continued support. I appreciate you going out of your way to optimize the code. Your insight has been very helpful for my implementation fmincon in optimal control problems in the future. Once again, I can't thank you enough.