ADD: code for task 10

1 files changed, 34 insertions, 1 deletions

diff --git a/templates/lqr_tuning.m b/templates/lqr_tuning.m
index 4a26158..a0394d4 100644
--- a/templates/lqr_tuning.m
+++ b/templates/lqr_tuning.m
@@ -8,4 +8,37 @@
 
 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;
+
+    tuning_struct = repmat(struct())
+    for i=1:size(Q,2)
+        ts = struct()
+
+        ts.InitialCondition = x0(:,i);
+        ts.Qdiag = Q(:,i);
+
+        ctrl = LQR(diag(Q(:,i)), eye(params.model.nu), params);
+        [u, ~] = ctrl.eval(x0(:,i))
+
+        [s_max, y_max, u_max, J_u, df_max, vf_max, traj_feas] = ...
+            traj_constraints(x0(:,i), u, params)
+
+        ts.MaxAbsPositionXZ = s_max;
+        ts.MaxAbsPositionY = y_max;
+        ts.MaxAbsThrust = u_max;
+        ts.InputCost = J_u;
+        ts.MaxFinalPosDiff = df_max;
+        ts.MaxFinalVelDiff = vf_max;
+        ts.TrajFeasible = traj_feas;
+
+        tuning_struct(i) = ts;
+
+        if ts.TrajFeasible
+            if ts.InputCost < best_J_u
+                i_opt = i;
+                best_J_u = ts.InputCost;
+            end
+        end
+    end
+end