FIX: pass lqr_tuning

1 files changed, 32 insertions, 21 deletions

diff --git a/templates/lqr_tuning.m b/templates/lqr_tuning.m
index a0394d4..9c9aab9 100644
--- a/templates/lqr_tuning.m
+++ b/templates/lqr_tuning.m
@@ -11,34 +11,45 @@ function [tuning_struct, i_opt] = lqr_tuning(x0,Q,params)
     i_opt = nan;
     best_J_u = inf;
 
-    tuning_struct = repmat(struct())
-    for i=1:size(Q,2)
-        ts = struct()
+    % Prepare an array of empty structs
+    tuning_struct = repmat(struct( ...
+        'InitialCondition', {}, ...
+        'Qdiag', {}, ...
+        'MaxAbsPositionXZ', {}, ...
+        'MaxAbsPositionY', {}, ...
+        'MaxAbsThrust', {}, ...
+        'InputCost', {}, ...
+        'MaxFinalPosDiff', {}, ...
+        'MaxFinalVelDiff', {}, ...
+        'TrajFeasible', {} ...
+    ), size(Q,2), 1);
 
-        ts.InitialCondition = x0(:,i);
-        ts.Qdiag = Q(:,i);
+    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);
-        [u, ~] = ctrl.eval(x0(:,i))
+        [Xt, Ut, ~] = simulate(x0, ctrl, params);
 
         [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
+            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 = ts.InputCost;
+                best_J_u = J_u;
             end
         end
     end
+
+    % because the test suite wants a column vector
+    tuning_struct = tuning_struct';
 end