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
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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 [params_z] = generate_params_z(params)
% initialize params_z
params_z = params;
% display(params.model);
% add initial condition of z-subsystem
params_z.model = rmfield(params_z.model,{'InitialConditionA', ...
'InitialConditionB', ...
'InitialConditionC'});
params_z.model.InitialConditionA_z = [-0.07;0.04]; % TODO: tune
% define projection matrices
T_z_x = [0 0 1 0 0 0;
0 0 0 0 0 1];
T_z_u = [0 0 1];
% projected system dynamics
params_z.model.A = T_z_x*params.model.A*T_z_x';
params_z.model.B = T_z_x*params.model.B*T_z_u';
params_z.model.nx = 2;
params_z.model.nu = 1;
% state constraints
Hx = params.constraints.StateMatrix * T_z_x';
hx = params.constraints.StateRHS;
X = Polyhedron('A',Hx,'b',hx);
X.minHRep();
params_z.constraints.StateMatrix = X.A;
params_z.constraints.StateRHS = X.b;
% input constraints
Hu = params.constraints.InputMatrix * T_z_u';
hu = params.constraints.InputRHS;
U = Polyhedron('A',Hu,'b',hu);
params_z.constraints.InputMatrix = U.A;
params_z.constraints.InputRHS = U.b;
% disturbance constraints
max_disturbance = 1e-4; % TODO: chose
params_z.constraints.MaxAbsDisturbance = max_disturbance;
H_w = kron(eye(params_z.model.nx),[1;-1]);
h_w = ones(2*params_z.model.nx, 1) * max_disturbance;
params_z.constraints.DisturbanceMatrix = H_w;
params_z.constraints.DisturbanceRHS = h_w;
end
|
