2 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
diff --git a/templates/lqr_tuning_script.mat b/templates/lqr_tuning_script.mat
new file mode 100644
index 0000000..af9681c
--- /dev/null
+++ b/templates/lqr_tuning_script.mat