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)

1 files changed, 7 insertions, 3 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