summaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
Diffstat (limited to '')
-rw-r--r--templates/LQR.m10
-rw-r--r--templates/lqr_tuning.m46
-rw-r--r--templates/lqr_tuning_script.m58
-rw-r--r--templates/lqr_tuning_script.matbin0 -> 2792 bytes
-rw-r--r--templates/simulate.m15
-rw-r--r--templates/traj_constraints.m28
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
new file mode 100644
index 0000000..af9681c
--- /dev/null
+++ b/templates/lqr_tuning_script.mat
Binary files differ
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
generated by cgit v1.2.1 (git 2.18.0) at 2024-10-16 20:54:32 +0200