From b6f865025eed2db716f2f853435855edb0db9e82 Mon Sep 17 00:00:00 2001
From: Nao Pross <np@0hm.ch>
Date: Wed, 24 May 2023 16:22:02 +0200
Subject: Take deliverables for uncontrained optimal control from npross

According to table 5 they are:

  - LQR
  - LQR/eval (contained in LQR.m)
  - simulate
  - traj_contraints
  - lqr_tuning
  - lqr_tuning_script (.m and its .mat output file)
---
 templates/lqr_tuning_script.m | 58 +++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 58 insertions(+)
 create mode 100644 templates/lqr_tuning_script.m

(limited to 'templates/lqr_tuning_script.m')

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
-- 
cgit v1.2.1