Data-Driven-Predictive-Control/Robust_MPC_polytopes.m at main · aalanwar/Data-Driven-Predictive-Control · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
% ZPC: run RMPC with model using polytopes
%
%
% Inputs:
%    none
%
% Outputs:
%    saved workspace
%
% Example:
%
% See also: ---

% Author:       Yvonne Stürz
% Written:      25-March-2021
% Last update:  ---
% Last revision:---

%------------- BEGIN CODE --------------


clear all
close all
rand('seed',4500);

% The system description is xk+1=Axk+Buk+Ewk,yk=Cxk
A = [-1 -4 0 0 0; 4 -1 0 0 0; 0 0 -3 1 0; 0 0 -1 -3 0; 0 0 0 0 -2];
B_ss = ones(5,1);
C = [1,0,0,0,0];
D = 0;
% define continuous time system
sys_c = ss(A,B_ss,C,D);
% convert to discrete system
samplingtime = 0.05;
sys_d = c2d(sys_c,samplingtime);
A = sys_d.A;
B = sys_d.B;

dim_x = 5;
C = eye(5);
E = eye(5);

wfac=0.01;
vfac = 0.002;
Qy = 1e3*eye(5); %define output cost matrix
Qu = 0.001*eye(1);%control cost matrix

uref = 8;
ref = inv(eye(5)-sys_d.A)*sys_d.B*uref;

y_lb = [-10;2;-10;-10;-10]; %[-2;2;1;-2.5;3];
y_ub = [10;10;10;10;10]; %[2;4;4;3;6];
y0 = [-2;4;3;-2.5;5.5];


% Open loop mini-max solution

N = 2;
U = sdpvar(N,1);
W = sdpvar(N,5);
V = sdpvar(N,5);
x = sdpvar(5,1);

Vw = wfac*ones(5,1);
Vw = [Vw'; -Vw'];
Pw = Polyhedron('V', Vw);
%plot(P1)
Pw.minHRep;
Pw.H;
Pw.He;

test = [0;0;0;0;0];
W_Constraints = [Pw.A * test <= Pw.b; Pw.Ae * test == Pw.be]


Vv = vfac*ones(5,1);
Vv = [Vv'; -Vv'];
Pv = Polyhedron('V', Vv);
%plot(P1)
Pv.minHRep;
Pv.H;
Pv.He;

test = [0;0;0;0;0];
V_Constraints = [Pv.A * test <= Pv.b; Pv.Ae * test == Pv.be]


Y = [];
xk = x;
for k = 1:N
 xk = A*xk + B*U(k)+E*W(k,:)';
 Y = [Y; C*xk + V(k,:)'];
end

F = [kron(ones(N,1),y_lb) <= Y <= kron(ones(N,1),y_ub), kron(ones(N,1),-32) <= U <= kron(ones(N,1),46)];
objective = norm(Y-kron(ones(N,1),ref),2)*Qy(1) + norm(U-kron(ones(N,1),uref),2)*Qu(1);

G = [];
for k = 1:N
    G = [G, blkdiag(Pw.A,Pv.A) * [W(k,:)'; V(k,:)'] <= [Pw.b; Pv.b], blkdiag(Pw.Ae,Pv.Ae) * [W(k,:)'; V(k,:)'] == [Pw.be;Pv.be]];
end


[Frobust,h] = robustify(F + G,objective,[],[W;V]);


xk = y0;
uk = [];
Y = y0;
ops = sdpsettings;
maxsteps = 80;
execTime=[];
for i = 1:maxsteps
    tic
    optimize([Frobust, x == xk(:,end)],h,ops);
    execTime=[execTime toc];
    xk = [xk A*xk(:,end) + B*value(U(1)) + E*wfac*(-1+2*rand(1)*ones(5,1))];
    Y = [Y, C*xk(:,end) + vfac*(-1+2*rand(1)*ones(5,1))];
    uk = [uk value(U(1))];
end

Cost_rob_ol_tot=0;
Cost_rob_ol=[];
for i = 1:maxsteps
    Cost_rob_ol = [Cost_rob_ol, (Y(:,i+1)-ref)'*Qy*(Y(:,i+1)-ref)+ (uk(:,i)-uref)'*Qu*(uk(:,i)-uref)];
    Cost_rob_ol_tot = Cost_rob_ol_tot + (Y(:,i+1)-ref)'*Qy*(Y(:,i+1)-ref)+ (uk(:,i)-uref)'*Qu*(uk(:,i)-uref);
    yt2ref_poly(i) = norm(Y(:,i)-ref,2);
end
Cost_rob_ol_tot
meanExecTime=mean(execTime)
stdExecTime= std(execTime)
% figure(1)
% %plot([C*xk  + vfac*(-1+2*rand(1)*ones(5,1))]')
% plot([Y]')
% hold on, plot(kron(ones(1,100),ref)')
% figure(2)
% %plot([C*xk  + vfac*(-1+2*rand(1)*ones(5,1))]')
% plot([Y]')
% hold on, plot(kron(ones(1,100),ref)')
% hold on, plot(kron(ones(1,100),y_lb)')
% hold on, plot(kron(ones(1,100),y_ub)')

save('workspaces\poly');