test.m

close all;
clear;
clc;
set(groot, 'defaultAxesTickLabelInterpreter','latex'); 
set(groot, 'defaultLegendInterpreter','latex');
%% state space water tank model
A = [0.9692 0 -0.0207 0; 0 0.9652 0 -0.0204; 0 0 0.9789 0; 0 0 0 0.9792];
B = [0.0068 -0.0001; 
    -0.0001 0.0091; 
    0 0.0137; 0.0137 0];
C = [eye(2,2),zeros(2,2)];
% C = [eye(2,2),zeros(2,2);eye(4)];

D = zeros(2,2)

% D = zeros(6,2)



[n,m] = size(B);

T_s = 15;
sys = ss(A,B,C,D,T_s)
%% weight for the optimization problem
q = 100*eye(size(A,1))
r = eye(m)


[~,P] = dlqr(A,B,q,r)



%% prediction horizon
N = 5;

%% init state
x_0 = [0.4 0.8 0.5 0.4]'
x_eq = [0.6270 0.6360 0.6520 0.6330]'
u_eq = [1.6429 2]'

%% steady state [x_eq,u_eq]
M = null([eye(n)-A, -B]) 

%% reference

rif1 = [0.1,0.15]'
dxu_r = M*rif1

dx_r = dxu_r(1:4)
du_r = dxu_r(5:6)

x_r = dx_r + x_eq
u_r = du_r + u_eq

X_r = getXr(dx_r,N)
U_r = getXr(du_r,N-1)

% u_min, u_max
u_min = [0 0]' - u_eq;
u_max = [3.26 4]' - u_eq;
U_min = getXr(u_min,N-1);
U_max = getXr(u_max,N-1);

%%

%Compute Gx 
Gx = Compute_Gx(A,N)
%Compute Gu
Gu = Compute_Gu(A,B,N)

%Compute  Q
Q = getQ(q,P,N)

%Compute R
R = getR(r,N)

%Define H and F
H = 2*(Gu' * Q * Gu + R)

% F1 = 2*x_0'*Gx'*Q*Gu

F2 = -2*X_r'*Q*Gu-2*U_r'*R


%% simulation
T_max = 200;
x = zeros(n,T_max);
u = zeros(m,T_max);
dx = zeros(n,T_max);
du = zeros(m,T_max);
x(:,1) = x_0;
dx(:,1) = x_0 - x_eq;

%%

for k = 1:T_max
        
     %after 100 steps,  reference change
     if k == 100
        x_rif1 = x_r;
        u_rif1 = u_r;
        rif2 = [0.75,0.5]'
        dxu_r = M*rif2;
        
        dx_r = dxu_r(1:4);
        du_r = dxu_r(5:6);
        
        x_r = dx_r + x_eq;
        u_r = du_r + u_eq;

        x_rif2 = x_r;
        u_rif2 = u_r;
        
        X_r = getXr(dx_r,N);
        U_r = getXr(du_r,N-1);

        F2 = -2*X_r'*Q*Gu-2*U_r'*R;
    end
    
    F1 = 2*dx(:,k)'*Gx'*Q*Gu;
    U = quadprog(H,F1+F2,[],[],[],[],U_min,U_max);
    du(:,k) = U(1:m);
    
    dx(:,k+1) = A*dx(:,k) + B*du(:,k);
    x(:,k+1) = dx(:,k+1) + x_eq;
    u(:,k) = du(:,k) + u_eq;
    y(:,k) = C*x(:,k);
    
    
end
%%
for i=1:100
    x_ref(1,i)=x_rif1(1);
    x_ref(2,i)=x_rif1(2);
    x_ref(3,i)=x_rif1(3);
    x_ref(4,i)=x_rif1(4);
end
for i=101:T_max+1
    x_ref(1,i)=x_rif2(1);
    x_ref(2,i)=x_rif2(2);
    x_ref(3,i)=x_rif2(3);
    x_ref(4,i)=x_rif2(4);
end
%% Plot
tiledlayout(2,1)
ax1=nexttile
set(gca,'FontSize',18)

grid on
hold on
plot([0:T_max],x(1,:),'r','LineWidth',3)
plot([0:T_max],x(2,:),'g','LineWidth',3)
plot([0:T_max],x(3,:),'b','LineWidth',3)
plot([0:T_max],x(4,:),'Color','magenta','LineWidth',3)

plot([0:T_max],x_ref(1,:),'r-.','LineWidth',3)
plot([0:T_max],x_ref(2,:),'g-.','LineWidth',3)
plot([0:T_max],x_ref(3,:),'b-.','LineWidth',3)
plot([0:T_max],x_ref(4,:),'Color','magenta','LineStyle','-.','LineWidth',3)

% plot([0:T_max],x_rif2(1)*ones(T_max+1),'r-.','LineWidth',3)
% plot([0:T_max],x_rif2(2)*ones(T_max+1),'g-.','LineWidth',3)
% plot([0:T_max],x_rif2(3)*ones(T_max+1),'b-.','LineWidth',3)
% plot([0:T_max],x_rif2(4)*ones(T_max+1),'Color','magenta','LineStyle','-.','LineWidth',3)
ylabel('H [$m$]', 'Interpreter','latex','FontSize',18)
 legend('$H_1$','$H_2$','$H_3$','$H_4$',...
     '${H{_1}}_{ref}$','${H{_2}}_{ref}$','${H{_3}}_{ref}$','${H{_4}}_{ref}$','Interpreter','latex','Location','southeast','NumColumns',2)

ax2=nexttile
set(gca,'FontSize',18)

grid on
hold on
plot([0:T_max-1],u(1,:),'r','LineWidth',3)
plot([0:T_max-1],u(2,:),'b','LineWidth',3)
ylabel('Q[$\frac{m^3}{h}$]', 'Interpreter','latex','FontSize',18)

xlabel('Steps', 'Interpreter','latex','FontSize',18)
legend('$Q_a$','$Q_b$','Interpreter','latex','Location','southeast','NumColumns',2)