Using fmincon to solve objective function in integral form.

Question

Joseph Criniti il 10 Apr 2024 alle 3:14

0
Link

Link diretto a questa domanda

https://it.mathworks.com/matlabcentral/answers/2105041-using-fmincon-to-solve-objective-function-in-integral-form

Commentato: Joseph Criniti il 19 Apr 2024 alle 20:06

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

2 Commenti
Mostra NessunoNascondi Nessuno

Dyuman Joshi il 10 Apr 2024 alle 6:27

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 il 10 Apr 2024 alle 6:57

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.

Accedi per commentare.

Accedi per rispondere a questa domanda.

Answer 1

Bruno Luong il 10 Apr 2024 alle 6:23

1
Link

Link diretto a questa risposta

https://it.mathworks.com/matlabcentral/answers/2105041-using-fmincon-to-solve-objective-function-in-integral-form#answer_1439076

Apri in MATLAB Online

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.

15 Commenti
Mostra 13 commenti meno recentiNascondi 13 commenti meno recenti

Bruno Luong il 10 Apr 2024 alle 7:14

Modificato: Bruno Luong il 10 Apr 2024 alle 9:54

Apri in MATLAB Online

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 il 10 Apr 2024 alle 16:34

Apri in MATLAB Online

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

Joseph Criniti il 10 Apr 2024 alle 23:24

Apri in MATLAB Online

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

Bruno Luong il 11 Apr 2024 alle 11:06

Modificato: Bruno Luong il 11 Apr 2024 alle 11:13

Apri in MATLAB Online

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

Joseph Criniti il 12 Apr 2024 alle 3:16

Apri in MATLAB Online

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 il 12 Apr 2024 alle 6:26

Modificato: Bruno Luong il 12 Apr 2024 alle 6:35

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 il 17 Apr 2024 alle 10:48

Modificato: Bruno Luong il 17 Apr 2024 alle 16:46

optimalcontrol_test.m

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 il 19 Apr 2024 alle 20:06

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.

Accedi per commentare.

Using fmincon to solve objective function in integral form.

2 Commenti
Mostra NessunoNascondi Nessuno

Risposta accettata

15 Commenti
Mostra 13 commenti meno recentiNascondi 13 commenti meno recenti

Più risposte (0)

Vedere anche

Categorie

Tag

Prodotti

Release

Community Treasure Hunt

Using fmincon to solve objective function in integral form.

2 Commenti Mostra NessunoNascondi Nessuno

Risposta accettata

15 Commenti Mostra 13 commenti meno recentiNascondi 13 commenti meno recenti

Più risposte (0)

Vedere anche

Categorie

Tag

Prodotti

Release

Community Treasure Hunt

2 Commenti
Mostra NessunoNascondi Nessuno

15 Commenti
Mostra 13 commenti meno recentiNascondi 13 commenti meno recenti