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