Model Predictive Control (MPC) for electromechanical system - matlab

Question

So I want to create an MPC controller for my seesaw-cart system. All the "grunt work" (getting equations of motion, state-space representation etc.) has been done, so I went into coding into MATLAB.
Here's the following:
clc; clear all; close all;
yalmip('clear')
%% Parameters
a = 0.116553; % height of center mass of the pendulum, [m]
b = 0; % position of the weight B on the vertical rod (not taken into consideration)
c = 0.180047; % height of the center of mass of the cart, [m]
mA = 4.839; % mass of the pendulum, [kg]
mB = 0; % not taken into consideration
mC = 1; % mass of the cart, [kg]
g = 9.81; % gravity factor, [m/s^2]
kappa = 0.1; % coefficient of the viscous damping in the rotational joint
J = 0.68; % moment of inertia of the pendulum, [kgm^2]
Ke = 0.077; % motor constant of the EM force, [Vs^-1]
Kt = 0.077; % proportional moment motor constant, [NmA^-1]
Ra = 2.6; % electrical resistance, [ohm]
p = 1/3.71; % motor gearbox ratio
r = 7.7*10^(-3); % effective radius of the motor shaft, [m]
%% Defining the continuous system
A = [0 1 0 0;
(a*mA*g+b*mB*g)/(J+mB*b^2), -kappa/(J+mB*b^2), -mC*g/(J+mB*b^2), -Ke*Kt/(mC*(J+mB*b^2)*Ra*p^2*r^2);
0 0 0 1;
(a*mA*g+b*mB*g)/(J+mB*b^2)-g, -kappa*c/(J+mB*b^2), -c*mC*g/(J+mB*b^2), -(J+mB*b^2+mC*c^2)*Ke*Kt/(mC*(J+mB*b^2)*Ra*p^2*r^2)];
B = [0;
Kt/(mC*(J+mB*b^2)*Ra*p^2*r^2);
0;
(J+mB*b^2+mC*c^2)*Kt/(mC*(J+mB*b^2)*Ra*p^2*r^2)];
C = eye(4); % check
D = 0;
sysC = ss(A, B, C, D);
%% Defining the discrete system
Ts = 10e-3; % sample time
sysD = c2d(sysC,Ts); % discrete time
Ad = sysD.A;
Bd = sysD.B;
%% Formulation of the MPC problem
[nx, nu] = size(B);
Q = eye(nx);
R = eye(nu);
N = 1000;
% Input constraints, maximum and minimum voltage
umin = -6;
umax = 6;
% Still have to find this
xmin = [-deg2rad(10); -deg2rad(50); -0.5; -10];
xmax = [deg2rad(10); deg2rad(50); 0.5; 10];
uVar = sdpvar(repmat(nu,1,N),ones(1,N));
xVar = sdpvar(repmat(nx,1,N+1),ones(1,N+1));
constraints = [];
objective = 0;
ops = sdpsettings('verbose',0,'solver','quadprog');
for k = 1:N
objective = objective + xVar{k}'*Q*xVar{k} + uVar{k}*R*uVar{k};
constraints = [constraints, xVar{k+1} == Ad*xVar{k} + Bd*uVar{k}];
constraints = [constraints , umin <= uVar{k} <= umax , xmin <= xVar{k+1} <= xmax];
end
controller = optimizer(constraints, objective,ops,xVar{1},[uVar{:}]);
%% Simulation of the linearized model
nSim = 1000;
x0 = [0; 0; 0; 0.05]; % initial values, check
time = 0:Ts:nSim*Ts;
x = zeros(nx,nSim+1);
u = zeros(nu,nSim);
x(:,1) = x0;
for i = 2:nSim+1
disp(strcat(['Sim iter: ', num2str(i-1)]));
uOpt = controller{x(:,i-1)};
u(:,i-1) = uOpt(:,1);
x(:,i) = Ad*x(:,i-1) + Bd*u(:,i-1);
end
%% Plot
figure;
subplot(4,1,1);
plot(time,x(1,:), 'LineWidth', 2, 'Color', 'red'); grid; ylabel('{\theta} (rad)'); title('States');
subplot(4,1,2);
plot(time,x(2,:), 'LineWidth', 2, 'Color', [0.6350, 0.0780, 0.1840]); grid; ylabel('$\dot{\theta}$ (rad/s)', 'Interpreter', 'latex');
subplot(4,1,3);
plot(time,x(3,:), 'LineWidth', 2, 'Color', [0.4940, 0.1840, 0.5560]); grid; ylabel('s (m)');
subplot(4,1,4);
plot(time,x(4,:), 'LineWidth', 2, 'Color',[0, 0.7, 0.9]); grid; ylabel('$\dot{s}$ (m/s)', 'Interpreter', 'latex'); xlabel('t (s)');
figure;
stairs(time(1:end-1),u, 'LineWidth', 2, 'Color',[1, 0.647, 0]); grid; ylabel('Ua (V)'); xlabel('t (s)'); title('Input');
So I was wondering for any suggestions on improvements. What can I do to make my regulator more robust?
Here are my outputs for this particular code:
Note: I'm using YALMIP for the optimization part of the MPC.

Answer 1

You lose feasibility after a few iterations,
sol =
struct with fields:
yalmipversion: '20210331'
matlabversion: '9.9.0.1524771 (R2020b) Update 2'
yalmiptime: 0.1159
solvertime: 0.2331
info: 'Infeasible problem (QUADPROG))'
problem: 1
K>> i
i =
4
Your MPC controller is thus badly tuned (too short horizon being the obvious start)
Also, your definition of u is weird as it has length 4 instead of length N (thus meaning you have two trailing variables in u which never are used, leading to them having value Nan when you look at them)
There are numerous MPC-examples in the tutorials which you should be able to use directly
https://yalmip.github.io/example/standardmpc/
YALMIP-specific question are better asked at
https://github.com/yalmip/YALMIP/discussions
https://groups.google.com/g/yalmip