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
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Copyright (c) 2023, Amon Lahr, Simon Muntwiler, Antoine Leeman & Fabian Flürenbrock Institute for Dynamic Systems and Control, ETH Zurich.
%
% All rights reserved.
%
% Please see the LICENSE file that has been included as part of this package.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [H_tube,h_tube,n_iter] = compute_minRPI(K_tube,params)
% YOUR CODE HERE
fprintf("\n")
% Compute closed loop, disturbance set and disturbance reachable sets
Ak = params.model.A + params.model.B*K_tube;
W = Polyhedron('A', params.constraints.DisturbanceMatrix, ...
'b', params.constraints.DisturbanceRHS);
Fi = @(i) Ak^i * W;
fprintf("Stable error dynamics? %d\n", all(eig(Ak) < 1))
fprintf("Decreasing DRS? %d\n", Fi(3).volume() > Fi(4).volume());
% Start algorithm
omega = Polyhedron.emptySet(params.model.nx);
n_iter = 0;
while n_iter < 200
Fn_iter = Fi(n_iter);
omega_next = omega + Fn_iter;
fprintf("n_iter=%d: volume=%d, fulldim=%d, bounded=%d\n", ...
n_iter, Fn_iter.volume(), omega_next.isFullDim(), omega_next.isBounded())
% why does this happen?
if ~ omega_next.isFullDim() || ~ omega_next.isBounded()
fprintf("There is something off ...\n");
break;
end
if eq(omega_next.minHRep(), omega.minHRep())
fprintf("converged!\n");
break;
end
n_iter = n_iter+1;
omega = omega_next;
end
H_tube = omega.A;
h_tube = omega.b;
end
|
