Problems encountered in solving a quadratic programming problem

Hi all,

I'm having a problem solving a quadratic programming problem with the following background: I approximated a single pendulum model to a linear model based on Koopman operator, and then based on that model, I solved a quadratic programming problem with kkt condition, and the goal is to get this pendulum upright and balanced (x0=[0;0], xf=[pi;0]), cost is energy optimal. The problem is: 1. The problem is: 1. if I set xf to [pi;0] and the optimisation time to 2 seconds, the result is wrong, if I set the optimisation time to 0.5 seconds, the answer is right again; 2. if I set xf to [pi/3;0], the answer is right no matter what the optimisation time is, the bigger the angle of the end point is, the more wrong the answer is, and it is basically wrong after exceeding pi/2. I hope you can help to analyse it, thanks!

Fig. end angle is 2pi，optimization time is 0.5s

Fig.2 end angle is 2pi，optimization time is 2s

Fig.3 end angle is pi/3，optimization time is 2s

Sorry for forgetting to provide the code, here it is：

clc;clear all;
%Parameters of the single pendulum system
g = 9.8;
L=2;
m = 1;
B_sys=[0; 1/(m*L*L)];

%Collection of data
X=[];           
Y=[];           
U=[];
dt = 0.01; %sampling interval
total_traj = 30;
step_num = 300;
for traj=1:total_traj
    x=rand(2,1)*2-1;             %Generate initial conditions
    for tick=1:step_num
        u = 8*cos(8*pi*dt*tick);
        X=[X x];
        x=step(x, g, L, B_sys, u, dt);  %Forward Euler
        Y=[Y x];
        U=[U u];
    end
end
%lift
s=2;                               %number of states
Norder=1;                          %Choose the order of the monomial lifting function
Xlift=monomials(X,Norder);
Ylift=monomials(Y,Norder);

Nlift = size(Xlift,1);              %The order of the original data after lifting 

%caculate A, B, C based on Koopman Operator

AB=Ylift*pinv([Xlift;U]);
A=AB(1:Nlift,1:Nlift);
B=AB(1:Nlift,Nlift+1:end);
C=X*pinv(Xlift);


%test trajectory for comparision
step_num=3000;
x_log_test=zeros(2,step_num);
u_log_test=zeros(1,step_num);
x_log_koopman=zeros(2,step_num);
t_log_test=zeros(1,step_num);
x=[0;0];                                %initial conditons
for tick=1:step_num
    u=rand()*8-4; 
    x_log_test(:,tick)=x;
    x=step(x, g, L, B_sys, u, dt);
    t_log_test(tick)=tick*dt;
    u_log_test(tick)=u;
end
x=[0;0];
for tick=1:step_num
    x_log_koopman(:,tick)=x(:,1);
    xlift=monomials(x,Norder);
    ylift=A*xlift+B*u_log_test(tick);
    x=C*ylift;
end
figure(1)
plot(t_log_test,x_log_koopman(1,:),'--',t_log_test,x_log_test(1,:),'-',t_log_test,x_log_koopman(2,:),'--',t_log_test,x_log_test(2,:),'-');
legend('koopman-x1','original system-x1','koopman-x2','original system-x2')
title('test trajectory')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%Quadratic Programming
%Optimisation parameters
N = 200;                        % number of intervals
t0 = 0;                         % initial time
T = 2;                          % final time
h = T / N;                      % time step
x0 = [0; 0];                    % initial condition
xf = [pi; 0];                    % final condition
t = linspace(0, T, N+1);


% % % Store parameters for the use of constraint and objective function
para.N = N;
para.t0 = t0;
para.T = T;
para.h = h;
para.x0 = x0;
para.xf = xf;

% % % System matrices
para.A=A;
para.B=B;
para.C=C;
para.AB=AB;
para.B_sys = B_sys;


% % % %% % %% % %% % %% % %% % %% % %% % %% % %% % %% % %% % %% % %% % %% % %% % %% % %% % %
%For a QP problem, J = 0.5*Z*H*Z, s.t. Aeq*Z=beq, Z = [X0lift;...;XNlift;u0;...uN-1]

% % % %% % %% % %% % %% % %% % %% % %% % %% % %% % %% % %% % %% % %% % %% % %% % %% % %% % %

% %           H for cost function
H_koopman =  zeros((N+1)*(Nlift)+N, (N+1)*(Nlift)+N);
for i = (N+1)*(Nlift) + 1 : (N+1)*(Nlift)+N
    H_koopman(i,i)=1;
end


Aeq1_koopman = zeros((Nlift), (N+1)*(Nlift)+N);
for p = 1:Nlift
    Aeq1_koopman(p,p) = 1;
end

Aeq2_koopman = zeros(size(C,1),(N+1)*(Nlift)+N);
Aeq2_koopman(1,N*(Nlift)+1:N*(Nlift)+(Nlift)) = para.C(1,:);
Aeq2_koopman(2,N*(Nlift)+1:N*(Nlift)+(Nlift)) = para.C(2,:);

for i = 1:N
    Aeq3_right_koopman((Nlift)*i-(Nlift-1):(Nlift)*i, i) = para.B; 
end
for i = 1:N
    Aeq3_left_koopman((Nlift)*i-(Nlift-1):(Nlift)*i, (Nlift)*i-(Nlift-1):(Nlift)*i) = para.A; 
    Aeq3_left_koopman((Nlift)*i-(Nlift-1):(Nlift)*i, (Nlift)*(i+1)-(Nlift-1):(Nlift)*(i+1)) = -eye(Nlift);
end
Aeq3_koopman = [Aeq3_left_koopman,Aeq3_right_koopman];

Aeq_koopman = [Aeq1_koopman;Aeq2_koopman;Aeq3_koopman];


x0_lift = monomials(x0,Norder);
beq_koopman = zeros(size(x0_lift,1)+size(xf,1)+N*(Nlift),1);
beq_koopman(1:Nlift,1) = x0_lift;
beq_koopman(Nlift+1:Nlift+s,1) = xf;
tic
%           KKT_Matrix

KKT_koopman = [H_koopman, Aeq_koopman.'; Aeq_koopman, zeros(size(Aeq_koopman, 1))];
rhs_koopman = [zeros(size(H_koopman,1),1);beq_koopman];                        
solution_koopman = KKT_koopman \ rhs_koopman;           %Calculation of optimal inputs based on KKT conditions
u_opt_koopman  = solution_koopman((Nlift)*(N+1)+1:(Nlift)*(N+1)+N); %Extraction of optimal inputs

%%%%%%%%%%%%%%%%%%%%%%Calculation of optimal inputs based on the original non-linear model
%  Initial guess
u0 = -(rand(N,1)*20);
%  Set up options for solver
options = optimoptions('fmincon', 'Display', 'Iter', 'Algorithm', 'interior-point', 'MaxIterations', 3000, 'MaxFunctionEvaluations', Inf);
tic
u_opt_origin = fmincon(@(u) objfun(u), u0, [], [], [], [], [], [], @(u) confun_origin(u , g, L, para), options);
toc

% % % Calculate state based on optimal control with koopman-model
x_opt_koopman = zeros(2, N + 1);
x_opt_koopman(:,1) = x0;
for i = 1:N
    xlift_opt=monomials(x_opt_koopman(:,i),Norder);
    ylift_opt=para.A*xlift_opt+para.B*u_opt_koopman(i);
    x_koopman=para.C*ylift_opt;
    x_opt_koopman(:,i+1)=x_koopman;
end

% % Calculate state based on optimal control with origin-model
x_opt_origin = zeros(2, N + 1);
x_opt_origin(:, 1) = x0;
for i = 1:N
    x_opt_origin(:, i + 1) = x_opt_origin(:, i) + h * state_dot(x_opt_origin(:, i), g, L, u_opt_origin(i), B_sys);
end
% 


% % % Plot results
figure(2)

plot(t(:,1:N), u_opt_koopman(:, 1),'bo',t(:,1:N), u_opt_origin(:, 1),'-');

legend('control-koopman','control-origin')
title('Control values')
xlabel('time (s)');
ylabel('u');

figure(3)
plot(t, x_opt_koopman(1, :),'o', t, x_opt_koopman(2, :),'o',t, x_opt_origin(1, :),'-', t, x_opt_origin(2, :),'-')
legend('x1-koopman', 'x2-koopman','x1-origin', 'x2-origin')
title('States')
xlabel('time (s)');
ylabel('states')

%foward-euler
function x_next_state = step(x, g, L, B_sys, u, dt)
f = [x(2);-g*sin(x(1))/L];
x_next_state=x+dt*(f+B_sys*u);
end
%system dynamic
function [x_dot] = state_dot(state, g, L, u, B)
    % This defines the ODE equation of the system
    f = [state(2);(-g*sin(state(1))) / L];
    x_dot = f + B * u;
end
%cost function
function [L] = objfun(u)
    L = sum(u.^2) / 2;
end
%confun for fmincon
function [c, ceq] = confun_origin(u, g, L, para)
    N = para.N;
    h = para.h;
    x0 = para.x0;
    xf = para.xf;

    B_sys = para.B_sys;
    % Integrate over time interval
    x = zeros(2, N + 1);
    x(:, 1) = x0;

    for i = 1:N
        x(:, i + 1) = x(:, i) + h * state_dot(x(:, i), g, L, u(i), B_sys);
    end

    % Final state constraint
    ceq = x(:, end) - xf;
    % no Inequality constraint
    c = [];
end
%lifting function
function [lifted] = monomials(x, Nlift)
    lifted = [];
    for i = 1:size(x, 2)
        xi = x(:, i); 
        col_lifted = [1];         
        for q = 1:Nlift
            for p = 0:q
                col_lifted = [col_lifted; xi(1)^(q-p) * xi(2)^p];
            end
        end        
        lifted = [lifted, col_lifted];
    end
    lifted(isnan(lifted)) = 0;
end

[Torsten]

The problem is very specific and complicated - so there is little hope that someone out there will be able to help. But with the information given, it's almost impossible.

[John D'Errico]

When you provide NO code at all, you need to hope that someone is willing to completely solve your assignment, writting the complete code. That is strongly against the goals of Answers. Honestly, I would hope that nobody does it.

Since all you gave us is a moderately vague description of a very specific problem, that means someone will need to read your mind.

If you want help, then make it possible to get help.

[Chenyu Lin]

Thanks for the heads up, I've added the code.

Answer 1

[Gautam]

Hello, Chenyu Lin

I understand that you have set up a QP problem to control the pendulum from an initial state (x_0 = [0; 0]) (hanging down and stationary) to a final state (x_f = [\pi; 0]) (upright and stationary), aiming for energy-optimal control. However, you're facing two issues:

1. When the optimization time is set to 2 seconds, the control solution fails to achieve the desired final state of (x_f = [\pi; 0]). However, reducing the optimization time to 0.5 seconds produces a correct solution.
2. When the desired final state is less ambitious (e.g., (x_f = [\pi/3; 0])), the solution is correct regardless of the optimization time. However, as the target angle increases beyond (\pi/2), the solution becomes increasingly inaccurate.

The following could be the possible reasons for the inaccuracies in the results

1. The linear model derived from the Koopman operator might be accurate only for small angles and short time horizons. For larger angles and longer durations, the linear approximation may not capture the true dynamics well.
2. A longer control horizon allows more time for the pendulum to be moved but also increases the complexity of the control problem. If the linear model does not accurately represent the system over this longer horizon, the optimization might yield incorrect solutions
3. The pendulum's dynamics are inherently nonlinear, especially for large angles. This nonlinearity is not fully captured by the linear model, particularly for angles close to or greater than pi/2, where the dynamics of the pendulum change significantly compared to the small-angle scenario

To get more accurate results, you can try using different linearization points or nonlinear control strategies that can directly handle the pendulum's nonlinear dynamics.

Thank You

Gautam Murthy