clc;
clear;
close all;
%% Time structure
t0 = 0;
tf =18;
dt = 0.1;
time = t0:dt:tf-dt;
%% System structure ===> x(k+1)=A*x(k)+B*u(k)
A = [1.02 -0.1
0.1 0.98];
B= [3.5; 2.6];
G=[0.3 0;0 0.3];
C=[1 0];
%% MPC parameters
Q =1.5975;
R = 37.0045;
N = 9;
M = blkdiag(Q,R);
w1 = randn(1,500);
w1 = w1-mean(w1);
w1 = w1/1;
sigmaw1 = var(w1);
w2 = randn(1,500);
w2 = w2-mean(w2);
w2 = w2/1;
sigmaw2 = var(w2);
w=[w1;w2];
sigmaw = [sigmaw1 0 ;0 sigmaw2];
v = randn(1,500);
v = v-mean(v);
v = v/1;
sigmav = var(v);
x = randn(2,1);
y=C*x+v(:,1);
sigmax{1} = [1 0;0 1];
sigmay{1}=C*sigmax{1}*C'+sigmav;
h1y = [1 ; 1];
h2u = 1;
h3u = -1;
epsilon =0.01;
g1 = 0.1;
g2 = 1;
g3 = 1;
%% Pre-allocation
U1 = zeros(1,180);
mux1 = zeros(1,180);
mux2 = zeros(1,180);
f1 = cell(1,180);
Y1 = cell(1,180);
K1 = cell(1,180);
u2 = cell(1,180);
muy1 = zeros(1,180);
y1 = zeros(1,180);
ud1 = zeros(1,180);
x1 = zeros(1,180);
x2 = zeros(1,180);
umax=zeros(1,180);
umin=zeros(1,180);
ymax=zeros(1,180);
%% Optimizer
mux = sdpvar(repmat(2,1,N),repmat(1,1,N)) ;
muy= sdpvar(repmat(1,1,N),repmat(1,1,N));
sigmax= sdpvar(repmat (2,1,N+1),repmat(2,1,N+1));
sigmay = sdpvar(repmat (1,1,N+1),repmat(1,1,N+1));
p = sdpvar(repmat(2,1,N-1),repmat(2,1,N-1) );
pN = sdpvar(1,1);
F = sdpvar(repmat(1,1,N+1),repmat(1,1,N+1) );
muu = sdpvar(repmat(1,1,N),repmat(1,1,N)) ;
sigmau = sdpvar(repmat(1,1,N),repmat(1,1,N) );
constraint = [];
objective = 0;
for k=1:N-1
mux{k+1}=A*mux{k}+B*muu{k};
muy{k+1}=C*mux{k+1};
objective = objective +1/2*(trace(M*p{k})) ;
constraint2 = [sigmay{k+1} C*A*sigmax{k} C*B*F{k} C*G*sigmaw sigmav
( C*A*sigmax{k})' sigmax{k} zeros(2,1) zeros(2,2) zeros(2,1)
(C*B*F{k})' zeros(1,2) sigmay{k} zeros(1,2) zeros(1)
(C*G*sigmaw)' zeros(2,2) zeros(2,1) sigmaw zeros(2,1)
sigmav zeros(1,2) zeros(1) zeros(1,2) sigmav]>=0;
constraint1 = [sigmax{k+1} A*sigmax{k} B*F{k} G*sigmaw
(A*sigmax{k})' sigmax{k} zeros(2,1) zeros(2)
(B*F{k})' zeros(1,2) sigmay{k} zeros(1,2)
G*sigmaw zeros(2,2) zeros(2,1) sigmaw]>=0;
constraint3 = [sigmau{k} (F{k});(F{k})' sigmay{k}]>=0;
constraint4 = h1y'*mux{k}<=(1-(0.5*epsilon))*g1-((0.95/2*epsilon*g1*(1-0.95))*h1y'*(sigmay{k})*h1y);
constraint5 = h2u'*muu{k}<=(1-(0.5*epsilon))*g2-((0.95/2*epsilon*g2*(1-0.95))*h2u'*sigmau{k}*h2u);
constraint6 = h3u'*muu{k}<=(1-(0.5*epsilon))*g3-((0.95/2*epsilon*g3*(1-0.95))*h3u'*sigmau{k}*h3u);
constraint7 = [p{k} [sigmay{k};F{k}] [muy{k};muu{k}]
([sigmay{k} ;F{k}])' sigmay{k} zeros(1)
([muy{k};muu{k}])' zeros(1) 1]>=0;
constraint = [constraint,constraint2,constraint1,constraint3,constraint4,constraint5,constraint6,constraint7];
end
objective=objective+(1/2*trace((trace(Q*pN))));
constraint10 = h1y'*(mux{N})<=(1-(0.5*epsilon))*g1-((0.95/2*epsilon*g1*(0.1))*h1y'*(C*sigmax{N}*C'+sigmav)*h1y);
constraint11 = [pN-(sigmay{k}) (muy{N});(muy{N})' 1] >=0;
constraint = [constraint,constraint10,constraint11];
option = sdpsettings('solver','mosek');
sol_out={mux{2},muu{1},F{1}, sigmay{1} , muy{1},mux{1},sigmax{2}};
a = optimizer (constraint,objective,option,mux{1},sol_out);
mux{1} = [-5;2];
%% Main loop
c=0;
for t=t0:dt:tf-dt
c=c+1;
sol=a(mux{1});
u = sol{2};
U1(c)=u(1,1);
mux3 = sol{6};
mux1(c) = mux3(1,1);
mux2(c) = mux3(2,1);
muy3 =muy{1};
F = sol{3};
Y = sol{4};
f1{c} = F;
Y1{c} = Y;
K=F/Y;
K1{c} = K;
u1 = u+K*((y)-(muy3));
u2{c} = u1;
x =A*x+B*u1 +G*w(:,c);
y = C*x+v(:,c);
mux{1} = sol{1};
sigmax{1} = sol{7};
muy{1}=C*mux{1};
sigmay{1}=C*sigmax{1}*C'+sigmav;
muy1(c)=muy3;
y1(c)=y;
ud = value(u2{c});
ud1(c) = ud(1,1);
x1(c) = x(1,1);
x2(c) = x(2,1) ;
umax(c)=1;
umin(c)=-1;
ymax(c)= 2;
end
figure(1)
subplot(4,1,1)
plot(time,ud1,time,umin,'.-.',time,umax,'.-','linewidth',2)
xlabel('Time')
ylabel('Control signal')
legend({'Controller signal','umin','umax'}, 'interpreter', 'latex', 'FontSize', 8)
legend show
grid on;
hold on
subplot(4,1,2)
plot(time,y1,time,muy1,'-.',time,ymax,'.-.','linewidth',2)
xlabel('Time')
legend({'$$y$$ Output', '$$\mu$$($${y}$$) Mean of output','ymax'}, 'interpreter', 'latex', 'FontSize', 8)
legend show;
grid on;
hold on
subplot(4,1,3)
plot(time,y1,'linewidth',2)
xlabel('Time')
ylabel('Output')
grid on;
hold on
subplot(4,1,4)
plot(mux1,mux2,'linewidth',2)
xlabel('Mean of 1st state variable')
ylabel('Mean of 2nd state variable')
grid on;
figure(2)
subplot(3,1,1)
plot(time,muy1,'linewidth',2)
xlabel('Time')
ylabel('Mean of output Variable')
grid on;
subplot(3,1,2)
plot(time,x1,time,mux1,'-.','linewidth',2)
xlabel('Time')
legend({'$$x_1$$ First state variable', '$$\mu$$($$x_1$$) Mean of first state variable'}, 'interpreter', 'latex', 'FontSize', 8)
grid on
hold on
subplot(3,1,3)
plot(time,x2,time,mux2,'-.','linewidth',2)
xlabel('Time')
legend({'$$x_2$$ Second state variable', '$$\mu$$($$x_2$$)Mean of second state variable '}, 'interpreter', 'latex', 'FontSize', 8)
grid on