From 239de879bffd8b890d1fed026cf748762bbe0362 Mon Sep 17 00:00:00 2001 From: Nao Pross Date: Wed, 3 May 2023 15:44:15 +0200 Subject: ADD: code for task 10 --- templates/lqr_tuning.m | 35 ++++++++++++++++++++++++++++++++++- 1 file changed, 34 insertions(+), 1 deletion(-) (limited to 'templates') 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 -- cgit v1.2.1