summaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
Diffstat (limited to '')
-rw-r--r--templates/LQR.m9
-rw-r--r--templates/generate_constraints.m7
-rw-r--r--templates/generate_params.m15
-rw-r--r--templates/generate_system.m8
-rw-r--r--templates/generate_system_cont.m15
-rw-r--r--templates/generate_system_scaled.m9
-rw-r--r--templates/simulate.m15
-rw-r--r--templates/traj_constraints.m27
8 files changed, 91 insertions, 14 deletions
diff --git a/templates/LQR.m b/templates/LQR.m
index 2a79da9..76d95c6 100644
--- a/templates/LQR.m
+++ b/templates/LQR.m
@@ -15,13 +15,16 @@ classdef LQR
%constructor
function obj = LQR(Q,R,params)
% YOUR CODE HERE
- % obj.K = ... (save feedback matrix for use in eval function)
+ E = np.zeros(params.model.nx);
+ S = np.zeros(params.model.nx);
+ Pinf = idare(params.model.A, params.model.B, Q, R, S, E, 'noscaling')
+ obj.K = -inv(R + B' * Pinf * B) * B' * Pinf * 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/generate_constraints.m b/templates/generate_constraints.m
index 892b706..e2e4357 100644
--- a/templates/generate_constraints.m
+++ b/templates/generate_constraints.m
@@ -7,5 +7,10 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [H_u, h_u, H_x, h_x] = generate_constraints(params)
- % YOUR CODE HERE
+ H_u = [eye(params.model.nu); -eye(params.model.nu)];
+ h_u = params.constraints.MaxAbsThrust * ones(params.model.nu*2,1);
+
+ H_x = [eye(3), zeros(3); -eye(3), zeros(3)]; % * inv(params.model.ScalingMatrix);
+ h_x = params.constraints.MaxAbsPositionXZ * [1, 0, 1, 1, 0, 1].' + ...
+ params.constraints.MaxAbsPositionY * [0, 1, 0, 0, 1, 0].';
end \ No newline at end of file
diff --git a/templates/generate_params.m b/templates/generate_params.m
index ef51366..4b61e31 100644
--- a/templates/generate_params.m
+++ b/templates/generate_params.m
@@ -40,6 +40,19 @@ params.exercise = struct( ...
'QdiagOptA', [94.0; 0.1579; 300; 0.01; 0.10; 0.10] ...
);
-% YOUR CODE HERE
+% Add system model
+[Ac, Bc] = generate_system_cont(params);
+[At, Bt] = generate_system(Ac, Bc, params);
+[A, B] = generate_system_scaled(At, Bt, params);
+
+params.model.A = A;
+params.model.B = B;
+
+% Add constraints
+[H_u, h_u, H_x, h_x] = generate_constraints(params);
+params.constraints.InputMatrix = H_u;
+params.constraints.InputRHS = h_u;
+params.constraints.StateMatrix = H_x;
+params.constraints.StateRHS = h_x;
end
diff --git a/templates/generate_system.m b/templates/generate_system.m
index 9deb347..b44bef2 100644
--- a/templates/generate_system.m
+++ b/templates/generate_system.m
@@ -7,5 +7,9 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [A, B] = generate_system(Ac, Bc, params)
- % YOUR CODE HERE
-end \ No newline at end of file
+ sys = ss(Ac, Bc, eye(params.model.nx), zeros(params.model.nx, params.model.nu));
+ sysd = c2d(sys, params.model.TimeStep);
+
+ A = sysd.A;
+ B = sysd.B;
+end
diff --git a/templates/generate_system_cont.m b/templates/generate_system_cont.m
index 2d3ee79..048ee7b 100644
--- a/templates/generate_system_cont.m
+++ b/templates/generate_system_cont.m
@@ -7,5 +7,16 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Ac, Bc] = generate_system_cont(params)
- % YOUR CODE HERE
-end \ No newline at end of file
+ % YOUR CODE HERE
+ omega_n_sq = params.model.GravitationalParameter / params.model.TargetRadius^3;
+ omega_n = sqrt(omega_n_sq);
+
+ Ac = [
+ zeros(3), eye(3);
+ 3*omega_n_sq, 0, 0, 0, 2*omega_n, 0;
+ 0, 0, 0, -2*omega_n, 0, 0;
+ 0, 0, -omega_n_sq, 0, 0, 0;
+ ];
+
+ Bc = [zeros(3); eye(3);] / params.model.Mass;
+end
diff --git a/templates/generate_system_scaled.m b/templates/generate_system_scaled.m
index eac8db8..d65c7b3 100644
--- a/templates/generate_system_scaled.m
+++ b/templates/generate_system_scaled.m
@@ -6,6 +6,11 @@
% Please see the LICENSE file that has been included as part of this package.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-function [A,B] = generate_system_scaled(At,Bt,params)
+function [A, B] = generate_system_scaled(At, Bt, params)
% YOUR CODE HERE
-end \ No newline at end of file
+ V = params.model.ScalingMatrix;
+ Vinv = inv(V);
+
+ A = V*At*Vinv;
+ B = V*Bt;
+end
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..c87bed6 100644
--- a/templates/traj_constraints.m
+++ b/templates/traj_constraints.m
@@ -8,5 +8,32 @@
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(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, ...
+ 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-11-23 12:19:49 +0100