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