summaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
authorNao Pross <np@0hm.ch>2023-05-03 17:34:46 +0200
committerNao Pross <np@0hm.ch>2023-05-03 17:35:29 +0200
commit33b10d0ee0c6cb72b10f3ed696c471f76fe99338 (patch)
treeb190a97e30814b97e77e05c65deffd6a8347f202
parentFIX: pass traj_constraints (diff)
downloadmpc_pe-33b10d0ee0c6cb72b10f3ed696c471f76fe99338.tar.gz
mpc_pe-33b10d0ee0c6cb72b10f3ed696c471f76fe99338.zip
FIX: pass lqr_tuning
-rw-r--r--templates/lqr_tuning.m53
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