diff options
author | Nao Pross <np@0hm.ch> | 2023-05-24 16:22:02 +0200 |
---|---|---|
committer | Nao Pross <np@0hm.ch> | 2023-05-24 16:32:02 +0200 |
commit | b6f865025eed2db716f2f853435855edb0db9e82 (patch) | |
tree | 0211e0d07543dcea8e8e6048ae58a3458f5ab2d9 /templates/lqr_tuning.m | |
parent | Take deliverables for system modelling from npross (diff) | |
download | mpc_pe-b6f865025eed2db716f2f853435855edb0db9e82.tar.gz mpc_pe-b6f865025eed2db716f2f853435855edb0db9e82.zip |
Take deliverables for uncontrained optimal control from npross
According to table 5 they are:
- LQR
- LQR/eval (contained in LQR.m)
- simulate
- traj_contraints
- lqr_tuning
- lqr_tuning_script (.m and its .mat output file)
Diffstat (limited to 'templates/lqr_tuning.m')
-rw-r--r-- | templates/lqr_tuning.m | 46 |
1 files changed, 45 insertions, 1 deletions
diff --git a/templates/lqr_tuning.m b/templates/lqr_tuning.m index 4a26158..9c9aab9 100644 --- a/templates/lqr_tuning.m +++ b/templates/lqr_tuning.m @@ -8,4 +8,48 @@ 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; + + % Prepare an array of empty structs + tuning_struct = repmat(struct( ... + 'InitialCondition', {}, ... + 'Qdiag', {}, ... + 'MaxAbsPositionXZ', {}, ... + 'MaxAbsPositionY', {}, ... + 'MaxAbsThrust', {}, ... + 'InputCost', {}, ... + 'MaxFinalPosDiff', {}, ... + 'MaxFinalVelDiff', {}, ... + 'TrajFeasible', {} ... + ), size(Q,2), 1); + + 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); + [Xt, Ut, ~] = simulate(x0, ctrl, params); + + [s_max, y_max, u_max, J_u, df_max, vf_max, traj_feas] = ... + 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 = J_u; + end + end + end + + % because the test suite wants a column vector + tuning_struct = tuning_struct'; +end |