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.m | 46 +++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 45 insertions(+), 1 deletion(-)

(limited to 'templates/lqr_tuning.m')

diff --git a/templates/lqr_tuning.m b/templates/lqr_tuning.m
index 4a26158..9c9aab9 100644
--- a/templates/lqr_tuning.m
+++ b/templates/lqr_tuning.m
@@ -8,4 +8,48 @@
 
 function [tuning_struct, i_opt] = lqr_tuning(x0,Q,params)
     % YOUR CODE HERE
-end
\ No newline at end of file
+    i_opt = nan;
+    best_J_u = inf;
+
+    % Prepare an array of empty structs
+    tuning_struct = repmat(struct( ...
+        'InitialCondition', {}, ...
+        'Qdiag', {}, ...
+        'MaxAbsPositionXZ', {}, ...
+        'MaxAbsPositionY', {}, ...
+        'MaxAbsThrust', {}, ...
+        'InputCost', {}, ...
+        'MaxFinalPosDiff', {}, ...
+        'MaxFinalVelDiff', {}, ...
+        'TrajFeasible', {} ...
+    ), size(Q,2), 1);
+
+    for i=1:size(Q,2)
+        tuning_struct(i).InitialCondition = x0;
+        tuning_struct(i).Qdiag = Q(:,i);
+
+        ctrl = LQR(diag(Q(:,i)), eye(params.model.nu), params);
+        [Xt, Ut, ~] = simulate(x0, ctrl, params);
+
+        [s_max, y_max, u_max, J_u, df_max, vf_max, traj_feas] = ...
+            traj_constraints(Xt, Ut, params);
+
+        tuning_struct(i).MaxAbsPositionXZ = s_max;
+        tuning_struct(i).MaxAbsPositionY = y_max;
+        tuning_struct(i).MaxAbsThrust = u_max;
+        tuning_struct(i).InputCost = J_u;
+        tuning_struct(i).MaxFinalPosDiff = df_max;
+        tuning_struct(i).MaxFinalVelDiff = vf_max;
+        tuning_struct(i).TrajFeasible = traj_feas;
+
+        if traj_feas
+            if J_u < best_J_u
+                i_opt = i;
+                best_J_u = J_u;
+            end
+        end
+    end
+
+    % because the test suite wants a column vector
+    tuning_struct = tuning_struct';
+end
-- 
cgit v1.2.1