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
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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] = generate_params()
params = struct();
Tf = 60*60*24 * 2; % = 2 days
dt = 60 * 10; % = 10 minutes
% model
params.model = struct(...
'nx', 6, ...
'nu', 3, ...
'Mass', 300, ...
'GravitationalParameter', 3.986e14, ...
'ScalingMatrix', [1e-6*eye(3), zeros(3); zeros(3), 1e-3*eye(3)], ...
'TargetRadius', 7000e3, ...
'TimeStep', dt, ...
'HorizonLength', ceil(Tf / dt), ...
'InitialConditionA', [-15e-3; -400e-3; 24.4e-3; 0; 0.0081; 0], ...
'InitialConditionB', [-20e-3; 400e-3; 24.4e-3; 0; 0.0108; 0], ...
'InitialConditionC', [0.02; 0.01; -0.005; 0; 0; 0] ...
);
% constraints
params.constraints = struct(...
'MaxAbsPositionXZ', 0.1, ...
'MaxAbsPositionY', 1, ...
'MaxAbsThrust', 1, ...
'MaxFinalPosDiff' , 3e-4, ...
'MaxFinalVelDiff', 1e-3 ...
);
params.exercise = struct( ...
'QdiagOptA', [94.0; 0.1579; 300; 0.01; 0.10; 0.10] ...
);
% YOUR CODE HERE
[Ac, Bc] = generate_system_cont(params);
[A_tilta, B_tilta] = generate_system(Ac, Bc, params);
[A, B] = generate_system_scaled(A_tilta, B_tilta, params);
[Hu, hu, Hx, hx] = generate_constraints(params);
params.model.A = A;
params.model.B = B;
params.constraints.InputMatrix = Hu;
params.constraints.InputRHS = hu;
params.constraints.StateMatrix = Hx;
params.constraints.StateRHS = hx;
% new_model_struct = struct(...
% 'A', A, ...
% 'B', B ...
% );
% new_constr_struct = struct(...
% 'InputMatrix', Hu,...
% 'InputRHS', hu,...
% 'StateMatrix', Hx,...
% 'StateRHS', hx ...
% )
% params.model = [params.model new_model_struct];
% params.constraints = [params.constraints, new_constr_struct];
end
|
