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
|
function [tuning_struct] = lqr_tuning_script(params)
% From the hint: look in the vicinity of vector from the next section
% also from the hint: qx, qy, qvx, qvy are decoupled from qz, qvz
q_start = [94, .1579, 300, .01, .1, .1];
% parameters for sampling
v = 0.4; % search around q +- v * q
nsamples = 3;
q = q_start;
best_J_u = inf;
while best_J_u > 7.6
% Create sampling grid
range = linspace(1-v, 1+v, nsamples)';
% Sample in qx qy qvx qvy
G = kron(q([1,2,4,5]), range);
[Sx, Sy, Svx, Svy] = ndgrid(G(:,1), G(:,2), G(:,3), G(:,4));
Q = [Sx(:), Sy(:), q(3) * ones(size(Sx(:))), ...
Svx(:), Svy(:), q(6) * ones(size(Sx(:)))]';
% Find best q
[ts, i_opt] = lqr_tuning(params.model.InitialConditionA, Q, params);
% Save new best solution
if ~ isnan(i_opt)
tuning_struct = ts(i_opt);
best_J_u = ts(i_opt).InputCost;
q = ts(i_opt).Qdiag';
end
% Sample in qz qvz
G = kron(q([3,6]), range);
[Sz, Svz] = ndgrid(G(:,1), G(:,2));
qs = kron(q, ones(size(Sz(:))));
Q = [qs(:,1:2), Sz(:), qs(:,3:4), Svz(:)]';
% Find best item
[ts, i_opt] = lqr_tuning(params.model.InitialConditionA, Q, params);
% Save new best solution
if ~ isnan(i_opt)
tuning_struct = ts(i_opt);
best_J_u = ts(i_opt).InputCost;
q = ts(i_opt).Qdiag';
end
% Search in a narrower region, increase resolution
v = v / 2;
% Log
fprintf("Lowest J_u so far: %d, searching with v=%f\n", best_J_u,v);
end
% Save for test
matfile = struct("tuning_struct", tuning_struct, "q", tuning_struct.Qdiag);
save("lqr_tuning_script.mat", "-struct", "matfile");
end
|
