diff options
Diffstat (limited to 'templates/lqr_tuning_script.m')
| -rw-r--r-- | templates/lqr_tuning_script.m | 58 |
1 files changed, 58 insertions, 0 deletions
diff --git a/templates/lqr_tuning_script.m b/templates/lqr_tuning_script.m new file mode 100644 index 0000000..4a02fc6 --- /dev/null +++ b/templates/lqr_tuning_script.m @@ -0,0 +1,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 |