ADD: Tasks 1-9

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