Model Predictive control with Delay

I wanted to simulate the control of a simple pendulum with model predictive control. In which there is a time delay (input delay) between controller and the plant and between plant and controller (feedback delay). And wanted to perform the simulation in matlab using codes only (custom MPC code) not in Simulink. I have created a code having input delay but the results are not comming good. When I run the code without any delay it is working perfectly but as soon as I introduces the delay it is behaving abnormally. Can anyone check my code given below and also help me in introducing the feedback delay? .

clear
clc
close all

g = 9.81;
l = 0.5;
m = 0.5;

% Continous time system
Ac = [0 1;(-g/l) 0];
Bc = [0; 1/(m*l*l)];
Cc = [1 0];
Dc = zeros(1,1);

% Sampling interval
del_t  = 0.1;

% Discreate time system
[Ad,Bd,Cd,Dd] = c2dm(Ac,Bc,Cc,Dc,del_t);


Np = 10; % prediction Horizon

T = 100;
tt= 0:1:T;
ind = length(tt);
x10 = -pi/3;
xf = pi/3;

th = ([]);
dth = ([]);
for i=1:ind
    t = tt(i);
    [th_d,dth_d]=trajectory(t,x10,xf,T,1);
    th(i) = th_d;
    dth(i) = dth_d;
end

x0 = [th(1);dth(1)];   % Initial Position
tu = 2;                % uplink delay 

Rp = th(2:ind); 
Rp(ind:ind+Np)= th(end);
Rp = Rp';
[Phi,F] = f_and_phi(Ad,Bd,Cd,Np);  % Pre-Computing matrics
n1 = length(F);
Q = 100*eye(n1);          % Weight Matrix for error minimization
R = 0.001*eye(Np);        % Weight Matrix for input regulation
H = 2*(Phi'*Q*Phi+R);     % Pre-computing Hessian Matrix

[n2,n3] = size(Bd);
ux = ([]);
y = ([]);
Y = th(1);
X = ([]);
rm = ([]);
ustr = ([]);

for i=1:length(tt)
    g = 2*(Phi'*Q'*F*x0-Phi'*Q'*Rp(i:Np+i-1));  %  Gradient Matrix
    u1 = quadprog(H,g);
    ustr(:,i) = u1; 
    y(i,1) = Y;
    if (i-tu)<=0
        u = 0; 
    else 
        u = ustr(1,i-tu);
    end
    disp(u)
    x = Ad*x0 + Bd*u;
    Y = Cd*x;
    x0 = x;
end
animate(tt,y,x10,xf)


function [Phi_CC,F] = f_and_phi(Ad,Bd,Cd,Np)
F = ([]);

for i=1:Np
    f(:,:,i) = Cd*Ad^(i);
    F = [F;f(:,:,i)];
end
Phi_CC = ([]);
Phi = ([]);
for j=1:Np
    for i =1:Np
    phi(:,:,i) = Cd*Ad^(i-1)*Bd;
    Phi = [Phi;phi(:,:,i)];
    end
    [n1,n2] = size(Cd*Bd);
    Phi_c(:,:,j) = [zeros(n1*j-n1,n2);Phi(1:n1*(Np-(j-1)),:)];
    
    Phi_CC = [Phi_CC Phi_c(:,:,j)];
end
end

function [th_d,dth_d]=trajectory(t,x0,xf,tf,n)

thin = x0;
thf = xf;
Tp=tf;
for i=1:n
    thi(i,1)=thin(i)+((thf(i)-thin(i))/Tp)*(t-(Tp/(2*pi))*sin((2*pi/Tp)*t));
    dthi(i,1)=((thf(i)-thin(i))/Tp)*(1-cos((2*pi/Tp)*t));
end

th_d=thi;
dth_d=dthi;
end 


function animate(tt,Y,x10,xf)
l = 0.5;
Xx = ([]);
Yy = ([]);
figure
for i=1:length(Y)
    t = tt(i);
    th0 = Y(i);
    Xx(i) = l*sin(th0);
    Yy(i) = -l*cos(th0);
    Xin = l*sin(x10);
    Yin = -l*cos(x10);
    Xf = l*sin(xf);
    Yf = -l*cos(xf);
    plot([0 Xx(i)],[0 Yy(i)],Xx,Yy,Xx(i),Yy(i),'*',0,0,'*',Xin,Yin,'o',Xf,Yf,'o','MarkerSize',10,'LineWidth',3)
    axis([-0.5 0.5 -0.55 0]) 
    xlabel('X Axis')
    ylabel('Y Axis')
    T = num2str(t);
    title('Current Sampling Instant = ',T)
    grid on 
    drawnow
end

Answer 1

[Yash]

Hi @Saurabh Chaudhary

I see that you have implemented a simple pendulum control system using Model Predictive Control (MPC) in MATLAB. To introduce feedback delay into your control system, you can use a state-space representation that accounts for feedback delay. Here's how you can modify your code to include feedback delay:

% previous code

% Feedback delay buffer
u_buffer = zeros(1, tu);

for i = 1:length(tt)
    g = 2 * (Phi' * Q' * F * x0 - Phi' * Q' * Rp(i:Np + i - 1));
    
    % Apply feedback delay
    if (i - tu) <= 0
        u = 0;
    else
        u = u_buffer(mod(i - tu - 1, tu) + 1); % Corrected line
    end
    
    % Store the current control input in the buffer
    u_buffer = [u_buffer(2:end), u];
    
    u1 = quadprog(H, g);
    ustr(:, i) = u1;
    
    y(i, 1) = Y;
    
    x = Ad * x0 + Bd * u;
    Y = Cd * x;
    x0 = x;
end

% code afterwards

The line that needed modification is the one where we retrieve the delayed control input from the "u_buffer". I've added the "mod" function to handle cases where the delay index becomes negative. This ensures that the code correctly accesses the delayed control input from the buffer.

I hope this helps.