summaryrefslogtreecommitdiffstats
path: root/templates
diff options
context:
space:
mode:
Diffstat (limited to 'templates')
-rw-r--r--templates/lqr_tuning.m35
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