Hello, I am designing an MPC controller for a 4th order plant, which has a 1x1 input and two outputs. In the first instance, this controller is using the quadprog solver for the optimization method. I would like to know if this is what I am doing. designing in the first instance is correct or is there some strange detail, I insert the code that I used for the script and a test of the simulink block used:
%%%%%%%%%% Sistema de control predictivo MPC QUAD PROG %%%%%%%%%%%%%%%
clc
clear all
close all
%La planta RFL se puede modelar como un sistema con respuesta oscilatoria
%de segundo orden debido a las vibraciones de la barra modelandose de la
%siguiente manera: J*x''+Bx'+Kx=0, con solución en laplace de la forma:
%X(s)= Xo/J*(s^2+(B/J)*s+K/J)^-1 y se iguala a la respuesta del sistema
%s^2=2*chi*w_n*s+w_n^2 y se tiene que w_n^2=K/J, 2*chi*w_n=B/J
%Especificaciones de la planta
%tiempo de seteo del servo ts<=0.5 [s], llega a estado estacionario
%porcentaje de overshoot inferior a 7.5%
%máximo angulo de deflexión de la barra |alfa |< 10 deg
%máximo voltaje de control |Vm| <=10 [V]
%% Parámetros de la planta RFL%
Beq=0.004; %Coeficiente de viscosidad de fricción equivalente en [N*m]/(rad/s)
Jeq=2.08e-3; %Momento de inercia equivalente en kg*m^2
ml=0.065; %Masa de la barra en kg
Ll=0.419; %Largo de la barra en m
w_n=19.4495; %Frecuencia de oscilacion en [rad/s]
Jl=(ml*Ll^2)/3; %Momento de inercia de la barra en kg*m^2 de la forma (ml*Ll^2)/3
Ks=Jl*w_n^2; %Fricción o dureza de la barra en N*m/rad
%% Parametros del filtro
% SRV02 filtro pasa altos en control PD utilizado para computar la
% velocidad theta y alfa
% Frecuencias de corte en (rad/s)
wcf_1 = 2*pi*50;
wcf_2 = 2*pi*10;
%% Matrices de la planta a representar en V.E.
A=[0 0 1 0; 0 0 0 1; 0 Ks/Jeq -Beq/Jeq 0; 0 -Ks*(Jl+Jeq)/(Jl*Jeq) Beq/Jeq 0];
B=[0;0;1/Jeq;-1/Jeq];
C=[1 0 0 0;0 1 0 0];
D=[0;0];
%% Se representa en variables de estado la planta RFL
sys_RFL=ss(A,B,C,D);
x_0 = [45;0;0;0]; %Condiciones iniciales, si hay efectos en alfa no hay solucion, es decir estado 2 y estado 4
%% Varables de estado en tiempo discreto
T_sample=2.02; %Tiempo de muestro en discreto
tz_RFL=c2d(sys_RFL,T_sample); %Conversión a tiempo discreto de la matriz
[Ad,Bd,Cd,Dd]=ssdata(tz_RFL); %Matrices en tiempo discreto
n=length(Ad); %Largo de la matriz A discreta
[L_1,P_1,pol_1] = dlqr(Ad,Bd,eye(4),eye(1));
%Ax=Ad-Bd*L_1;
%% Constantes y Restricciones
N=5; %Horizonte de prediccion
x_min= [-90;-8;-0;-0]; %Restricciones de estado mínimas
x_max= [90;8;0;0]; %Restricciones de estado máximas
u_min= -10; %Restricciones de entrada mínimas
u_max= 10; %Restricciones de entrada mínimas
x_Nmin= x_min; %Restricciones de estado mínimas
x_Nmax= x_max; %Restricciones de estado máximas
Ipos= eye(N); %a los estados
Ineg= -eye(N); %a los estados
Q=C'*C; %Matriz de peso de estado Q=C'*C
R=eye(size(B,2)); %Matriz de peso actuación 1 |Posiblemente haya que ajustar los valores
[L_inf, PN, ~] = dlqr(Ad,Bd,Q,R);
and the test code:
options = optimset('Display','off');
%%%Condiciones iniciales
x0=x_0;
%%%Determinando las dimenciones del sistema
n = length(A); % largo de A
dimA = size(A,1);
dimB = size(B,2);
uvec = zeros(dimB*N,1);
coder.extrinsic('quadprog') % Utiliza quadprog
%% Calculo de la matriz Oa y Aa
%%%Inicializando las matrices Oa y Aa que definen el costo de MPC
Oa = zeros(dimA*N,dimB*N);
Aa = zeros(dimA*N,dimA);
for k = 1:N
Oa(n*k-(n-1):n*k,1) = (A^(k-1))*B; %Se define la primera columna de la matriz Oa
end
for j = 2:N
for k = j:N
Oa(n*k-(n-1):n*k,j) = Oa(n*(k-1)-(n-1):n*(k-1),j-1); %Desde la segunda columna de la matriz Oa
end
end
for k = 1:N
Aa(n*k-(n-1):n*k,:) = A^k ; %Se define la matriz Aa
end
Aa_N = A^N;
Oa_N = Oa(end-3:end,:); %Últimos 4 estados de Oa
%% Matrices y Restricciones
%%%Calculo de H y F para el costo de MPC
H = 2*(kron(eye(N),R)+Oa'*blkdiag(kron(eye(N-1),Q),P_1)*Oa);
H = (H+H')/2;
h = ((2*x0'*Aa'*blkdiag(kron(eye(N-1),Q),P_1)*Oa)');
%%%Restricciones ¿de la señal de estado
%%%Matriz M
Oa_Nineq = [Oa_N;-Oa_N];
Aineq = [Oa;-Oa];
%%%Matriz c
ba1_N = x_Nmax-Aa_N*x0;
ba2_N = -x_Nmin+Aa_N*x0;
Ba_Nineq = [ba1_N;ba2_N];
Bineq = [repmat(x_max,N,1)-Aa*x0;repmat(-x_min,N,1)+Aa*x0];
%%% Restricciones de la señal de entrada
LB = u_min*ones(N,1);
UB = u_max*ones(N,1);
%% Restricciones agrupadas
G = [Oa_Nineq;Aineq;Ipos;Ineg];
g = [Ba_Nineq;Bineq;UB;-LB];
%%%Vector de salida
uvec = quadprog(H,h,G,g,[],[],[],[],[],options); % resuelve con quadprog
u = uvec(1);
I do this for a plant without reference, as long as it goes to state zero with the calculated input