diff options
| -rw-r--r-- | templates/LQR.m | 10 | ||||
| -rw-r--r-- | templates/lqr_tuning.m | 46 | ||||
| -rw-r--r-- | templates/lqr_tuning_script.m | 58 | ||||
| -rw-r--r-- | templates/lqr_tuning_script.mat | bin | 0 -> 2792 bytes | |||
| -rw-r--r-- | templates/simulate.m | 15 | ||||
| -rw-r--r-- | templates/traj_constraints.m | 28 |
6 files changed, 150 insertions, 7 deletions
diff --git a/templates/LQR.m b/templates/LQR.m index 2a79da9..873546a 100644 --- a/templates/LQR.m +++ b/templates/LQR.m @@ -15,13 +15,17 @@ classdef LQR %constructor function obj = LQR(Q,R,params) % YOUR CODE HERE - % obj.K = ... (save feedback matrix for use in eval function) + E = zeros(params.model.nx); + S = zeros(params.model.nx, params.model.nu); + Pinf = idare(params.model.A, params.model.B, Q, R); + obj.K = -inv(R + params.model.B' * Pinf * params.model.B) ... + * params.model.B' * Pinf * params.model.A; end function [u, ctrl_info] = eval(obj,x) % YOUR CODE HERE - % u = ... + u = obj.K * x; ctrl_info = struct('ctrl_feas',true); end end -end
\ No newline at end of file +end 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 diff --git a/templates/lqr_tuning_script.m b/templates/lqr_tuning_script.m new file mode 100644 index 0000000..4a02fc6 --- /dev/null +++ b/templates/lqr_tuning_script.m @@ -0,0 +1,58 @@ +function [tuning_struct] = lqr_tuning_script(params) + % From the hint: look in the vicinity of vector from the next section + % also from the hint: qx, qy, qvx, qvy are decoupled from qz, qvz + q_start = [94, .1579, 300, .01, .1, .1]; + + % parameters for sampling + v = 0.4; % search around q +- v * q + nsamples = 3; + + q = q_start; + best_J_u = inf; + while best_J_u > 7.6 + % Create sampling grid + range = linspace(1-v, 1+v, nsamples)'; + + % Sample in qx qy qvx qvy + G = kron(q([1,2,4,5]), range); + [Sx, Sy, Svx, Svy] = ndgrid(G(:,1), G(:,2), G(:,3), G(:,4)); + Q = [Sx(:), Sy(:), q(3) * ones(size(Sx(:))), ... + Svx(:), Svy(:), q(6) * ones(size(Sx(:)))]'; + + % Find best q + [ts, i_opt] = lqr_tuning(params.model.InitialConditionA, Q, params); + + % Save new best solution + if ~ isnan(i_opt) + tuning_struct = ts(i_opt); + best_J_u = ts(i_opt).InputCost; + q = ts(i_opt).Qdiag'; + end + + % Sample in qz qvz + G = kron(q([3,6]), range); + [Sz, Svz] = ndgrid(G(:,1), G(:,2)); + qs = kron(q, ones(size(Sz(:)))); + Q = [qs(:,1:2), Sz(:), qs(:,3:4), Svz(:)]'; + + % Find best item + [ts, i_opt] = lqr_tuning(params.model.InitialConditionA, Q, params); + + % Save new best solution + if ~ isnan(i_opt) + tuning_struct = ts(i_opt); + best_J_u = ts(i_opt).InputCost; + q = ts(i_opt).Qdiag'; + end + + % Search in a narrower region, increase resolution + v = v / 2; + + % Log + fprintf("Lowest J_u so far: %d, searching with v=%f\n", best_J_u,v); + end + + % Save for test + matfile = struct("tuning_struct", tuning_struct, "q", tuning_struct.Qdiag); + save("lqr_tuning_script.mat", "-struct", "matfile"); +end diff --git a/templates/lqr_tuning_script.mat b/templates/lqr_tuning_script.mat Binary files differnew file mode 100644 index 0000000..af9681c --- /dev/null +++ b/templates/lqr_tuning_script.mat diff --git a/templates/simulate.m b/templates/simulate.m index 3138cd1..6be40c3 100644 --- a/templates/simulate.m +++ b/templates/simulate.m @@ -7,8 +7,17 @@ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function [Xt,Ut,ctrl_info] = simulate(x0, ctrl, params) + % YOUR CODE HERE + % Hint: you can access the control command with ctrl.eval(x(:,i)) -% YOUR CODE HERE -% Hint: you can access the control command with ctrl.eval(x(:,i)) + Xt = zeros(params.model.nx, params.model.HorizonLength+1); + Ut = zeros(params.model.nu, params.model.HorizonLength); + ctrl_info = repmat(struct('ctrl_feas',true), 1, params.model.HorizonLength); -end
\ No newline at end of file + Xt(:,1) = x0; + for k=1:params.model.HorizonLength + [u, ~] = ctrl.eval(Xt(:,k)); + Ut(:,k) = u; + Xt(:,k+1) = params.model.A * Xt(:,k) + params.model.B * Ut(:,k); + end +end diff --git a/templates/traj_constraints.m b/templates/traj_constraints.m index 4b0a081..69d19d3 100644 --- a/templates/traj_constraints.m +++ b/templates/traj_constraints.m @@ -8,5 +8,33 @@ function [s_max, y_max, u_max, J_u, df_max, vf_max, traj_feas] = traj_constraints(x,u,params) % YOUR CODE HERE + + s_max = max(max(abs(x([1,3],:)))); + y_max = max(abs(x(2,:))); + u_max = max(max(abs(u))); + + J_u = 0; + for k=1:size(u,2) + J_u = J_u + u(:,k)' * u(:,k); + end + + df_max = sqrt(sum(x(1:3,end).^2)); + vf_max = sqrt(sum(x(4:6,end).^2)); + + constr = [ + s_max <= params.constraints.MaxAbsPositionXZ, ... + y_max <= params.constraints.MaxAbsPositionY, ... + u_max <= params.constraints.MaxAbsThrust, ... + df_max <= params.constraints.MaxFinalPosDiff, ... + vf_max <= params.constraints.MaxFinalVelDiff + ]; + + % disp([s_max, y_max, df_max, vf_max]) + % disp([params.constraints.MaxAbsPositionXZ, ... + % params.constraints.MaxAbsPositionY, ... + % params.constraints.MaxFinalPosDiff, ... + % params.constraints.MaxFinalVelDiff]) + % disp(constr') + traj_feas = all(constr); end |