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
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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
i_opt = nan;
best_J_u = inf;
tuning_struct = repmat(struct())
for i=1:size(Q,2)
ts = struct()
ts.InitialCondition = x0(:,i);
ts.Qdiag = Q(:,i);
ctrl = LQR(diag(Q(:,i)), eye(params.model.nu), params);
[u, ~] = ctrl.eval(x0(:,i))
[s_max, y_max, u_max, J_u, df_max, vf_max, traj_feas] = ...
traj_constraints(x0(:,i), u, params)
ts.MaxAbsPositionXZ = s_max;
ts.MaxAbsPositionY = y_max;
ts.MaxAbsThrust = u_max;
ts.InputCost = J_u;
ts.MaxFinalPosDiff = df_max;
ts.MaxFinalVelDiff = vf_max;
ts.TrajFeasible = traj_feas;
tuning_struct(i) = ts;
if ts.TrajFeasible
if ts.InputCost < best_J_u
i_opt = i;
best_J_u = ts.InputCost;
end
end
end
end
|
