Take deliverables for uncontrained optimal control from npross

According to table 5 they are:

  - LQR
  - LQR/eval (contained in LQR.m)
  - simulate
  - traj_contraints
  - lqr_tuning
  - lqr_tuning_script (.m and its .mat output file)

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
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