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
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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 [tuning_struct, i_opt] = lqr_tuning(x0,Q,params)
% YOUR CODE HERE
R = eye(params.model.nu);
size_Q = size(Q);
param_choices = size_Q(2);
J_array = [];
tuning_struct = [];
for i = 1:param_choices
Q_mat = diag(Q(:,i));
ctrl = LQR(Q_mat,R,params);
[Xt, Ut, ~] = simulate(x0, ctrl, params);
[s_max, y_max, u_max, J_u, df_max, vf_max, traj_feas] = traj_constraints(Xt,Ut,params);
% YOUR CODE HERE
% ctrl_feas_arr = [];
% for k = 1:params.model.HorizonLength
% ctrl_feas_arr = [ctrl_feas_arr,ctrl_info(k).ctrl_feas];
% end
% if all(ctrl_feas_arr)
% J = 0;
% for j = 1:params.model.HorizonLength
% J = J + Xt(:,j)'*Q_mat*Xt(:,j);
% J = J + Ut(:,j)'*R*Ut(:,j);
% end
% J = J + Xt(:,end)'*Q_mat*Xt(:,end);
% J_array = [J_array, J];
% tuning_struct_item.TrajFeasible = true;
% else
% tuning_struct_item.TrajFeasible = false;
% end
tuning_struct_item.InitialCondition = x0;
tuning_struct_item.Qdiag = Q(:,i);
tuning_struct_item.MaxAbsPositionXZ = s_max;
tuning_struct_item.MaxAbsPositionY = y_max;
tuning_struct_item.MaxAbsThrust = u_max;
tuning_struct_item.InputCost = J_u;
tuning_struct_item.MaxFinalPosDiff = df_max;
tuning_struct_item.MaxFinalVelDiff = vf_max;
tuning_struct_item.TrajFeasible = traj_feas;
if traj_feas
J_array = [J_array, J_u];
end
tuning_struct = [tuning_struct,tuning_struct_item];
end
% display(J_array)
tuning_struct = tuning_struct';
if isempty(J_array)
i_opt = nan;
else
[~, i_opt] = min(J_array);
end
end
|
