diff options
| -rw-r--r-- | templates/lqr_tuning.m | 35 |
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 |