Merge pull request #1 from YanzhenXiangRobotics/submissionHEADmaster

Submission

29 files changed, 826 insertions, 34 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/MPC.m b/templates/MPC.m
index 3e9d2f1..355eb81 100644
--- a/templates/MPC.m
+++ b/templates/MPC.m
@@ -15,31 +15,57 @@ classdef MPC
         function obj = MPC(Q,R,N,params)
             nu = params.model.nu;
             nx = params.model.nx;
-
-            % define optimization variables
+            % YOUR CODE HERE
+            A=params.model.A;
+            B=params.model.B;
             U = sdpvar(repmat(nu,1,N),ones(1,N),'full');
+            X = sdpvar(repmat(nx,1,N+1),ones(1,N+1),'full');
+
+            [K,P,~] = dlqr(A,B,Q,R);
+
+%             s_max=params.constraints.MaxAbsPositionXZ;
+%             y_max=params.constraints.MaxAbsPositionY;
+%             u_max = params.constraints.MaxAbsThrust;
+            H_x = params.constraints.StateMatrix;
+            h_x = params.constraints.StateRHS;
+            H_u = params.constraints.InputMatrix;
+            h_u = params.constraints.InputRHS;
             X0 = sdpvar(nx,1,'full');
+            objective = 0;
+            constraints = X{1} == X0;
+            for k = 1:N
+                constraints = [ ...
+                    constraints, ...
+                    X{k+1} == A*X{k} + B*U{k} , ...
+                    H_x * X{k} <= h_x, ...
+                    H_u * U{k} <= h_u ...
+                ];
 
-            % YOUR CODE HERE
-            
+                objective = objective + X{k}'*Q*X{k} + U{k}'*R*U{k};
+            end
+            objective=objective+X{N+1}'*P*X{N+1};
+            % terminal constraint
+%             constraints = [ ...
+%                 constraints, ...
+%                 X{N+1} == zeros(nx,1)
+%             ];
             opts = sdpsettings('verbose',1,'solver','quadprog');
             obj.yalmip_optimizer = optimizer(constraints,objective,opts,X0,{U{1} objective});
         end
 
         function [u, ctrl_info] = eval(obj,x)
-            %% evaluate control action by solving MPC problem, e.g.
+            % evaluate control action by solving MPC problem
             tic;
-            [optimizer_out,errorcode] = obj.yalmip_optimizer(x);
+            [optimizer_out,errorcode,~] = obj.yalmip_optimizer{x};
             solvetime = toc;
-            
+
             [u, objective] = optimizer_out{:};
 
             feasible = true;
             if (errorcode ~= 0)
                 feasible = false;
             end
-
             ctrl_info = struct('ctrl_feas',feasible,'objective',objective,'solvetime',solvetime);
         end
     end
-end
\ No newline at end of file
+end
diff --git a/templates/MPC_TE.m b/templates/MPC_TE.m
index e0b55d2..747c7ee 100644
--- a/templates/MPC_TE.m
+++ b/templates/MPC_TE.m
@@ -15,7 +15,33 @@ classdef MPC_TE
         function obj = MPC_TE(Q,R,N,params)
             % YOUR CODE HERE
             opts = sdpsettings('verbose',1,'solver','quadprog');
-            obj.yalmip_optimizer = optimizer(constraints,objective,opts,X0,{U{1} objective});
+
+            % Create yalmip variables
+            U = sdpvar(repmat(params.model.nu,N,1),ones(1,N));
+            X = sdpvar(repmat(params.model.nx,N+1,1),ones(1,N+1));
+
+            % Build cost function and constraints
+            objective = 0;
+            constraints = [];
+            for k=1:N+1
+                if k <= N
+                    % These constraints are for the index in 1, ..., N
+                    objective = objective + X{k}' * Q * X{k} + U{k}' * R * U{k};
+                    constraints = [constraints, ...
+                        X{k+1} == params.model.A * X{k} + params.model.B * U{k}, ...
+                        params.constraints.InputMatrix * U{k} <= params.constraints.InputRHS, ...
+                    ];
+                end
+
+                % These contrains are for 1, ..., N+1
+                constraints = [constraints, ...
+                    params.constraints.StateMatrix * X{k} <= params.constraints.StateRHS, ...
+                ];
+            end
+            % Terminal constraint
+            constraints = [constraints, X{N+1} == zeros(size(X{N+1}))];
+
+            obj.yalmip_optimizer = optimizer(constraints,objective,opts,X{1},{U{1} objective});
         end
 
         function [u, ctrl_info] = eval(obj,x)
diff --git a/templates/MPC_TS.m b/templates/MPC_TS.m
index 05f92ff..cbe7b45 100644
--- a/templates/MPC_TS.m
+++ b/templates/MPC_TS.m
@@ -15,8 +15,34 @@ classdef MPC_TS
         function obj = MPC_TS(Q,R,N,H,h,params)
             % YOUR CODE HERE
             opts = sdpsettings('verbose',1,'solver','quadprog');
-            obj.yalmip_optimizer = optimizer(constraints,objective,opts,X0,{U{1} objective});
-        end
+
+            % Create yalmip optimization variables
+            U = sdpvar(repmat(params.model.nu,N,1),ones(1,N));
+            X = sdpvar(repmat(params.model.nx,N+1,1),ones(1,N+1));
+
+            % Build cost function and constraints
+            Pinf = idare(params.model.A, params.model.B, Q, R);
+            objective = X{N+1}' * Pinf * X{N+1};
+            constraints = [];
+            for k=1:N+1
+                if k <= N
+                    % These constraints are for the index in 1, ..., N
+                    objective = objective + X{k}' * Q * X{k} + U{k}' * R * U{k};
+                    constraints = [constraints, ...
+                        X{k+1} == params.model.A * X{k} + params.model.B * U{k}, ...
+                        params.constraints.InputMatrix * U{k} <= params.constraints.InputRHS, ...
+                    ];
+                end
+
+                % These contrains are for 1, ..., N+1
+                constraints = [constraints, ...
+                    params.constraints.StateMatrix * X{k} <= params.constraints.StateRHS, ...
+                ];
+            end
+            % Terminal constraint
+            constraints = [constraints, H*X{N+1} <= h];
+
+            obj.yalmip_optimizer = optimizer(constraints,objective,opts,X{1},{U{1} objective});        end
 
         function [u, ctrl_info] = eval(obj,x)
             %% evaluate control action by solving MPC problem, e.g.
diff --git a/templates/MPC_TS_SC.m b/templates/MPC_TS_SC.m
index 0e64767..fb1c720 100644
--- a/templates/MPC_TS_SC.m
+++ b/templates/MPC_TS_SC.m
@@ -14,22 +14,58 @@ classdef MPC_TS_SC
     methods
         function obj = MPC_TS_SC(Q,R,N,H,h,S,v,params)            
             % YOUR CODE HERE
+            % initialize parameters
+            nu = params.model.nu;
+            nx = params.model.nx;
+            A=params.model.A;
+            B=params.model.B;
+            H_x = params.constraints.StateMatrix;
+            h_x = params.constraints.StateRHS;
+            H_u = params.constraints.InputMatrix;
+            h_u = params.constraints.InputRHS;
+            [~,P,~] = dlqr(A,B,Q,R);
+
+            U = sdpvar(repmat(nu,1,N),ones(1,N),'full');
+            X = sdpvar(repmat(nx,1,N+1),ones(1,N+1),'full');
+            sv = sdpvar(repmat(length(S(1,:)),1,N+1),ones(1,N+1),'full');
+
+            X0 = sdpvar(nx,1,'full');
+            objective = 0;
+            constraints = X{1} == X0;      
+            % items for 1 to N
+            for k=1:N
+                objective = objective + X{k}' * Q * X{k} + U{k}' * R * U{k} + sv{k}' * S * sv{k} + v * max(sv{k});
+                constraints = [constraints, ...
+                    X{k+1} == A * X{k} + B * U{k}, ...
+                    H_x * X{k} <= h_x+sv{k}, ...
+                    H_u * U{k} <= h_u, ...
+                    sv{k} >= 0 ...
+                ];
+            end
+            % items for N+1
+                objective = objective + X{N+1}'*P*X{N+1} + sv{N+1}'*S*sv{N+1} + v*max(sv{N+1});
+                constraints = [constraints, ...
+                    sv{N+1} >= 0, ...
+                    H_x * X{N+1} <= h_x + sv{N+1} ...
+                ];
+            % maximum positively invariant set constraint
+            constraints = [constraints, H * X{N+1} <= h];
             opts = sdpsettings('verbose',1,'solver','quadprog','quadprog.TolFun',1e-8);
             obj.yalmip_optimizer = optimizer(constraints,objective,opts,X0,{U{1} objective});
         end
 
         function [u, ctrl_info] = eval(obj,x)
-            %% evaluate control action by solving MPC problem, e.g.
+            % evaluate control action by solving MPC problem
             tic;
-            [optimizer_out,errorcode] = obj.yalmip_optimizer(x);
+            [optimizer_out,errorcode,~] = obj.yalmip_optimizer{x};
             solvetime = toc;
+
             [u, objective] = optimizer_out{:};
 
             feasible = true;
             if (errorcode ~= 0)
                 feasible = false;
             end
-
             ctrl_info = struct('ctrl_feas',feasible,'objective',objective,'solvetime',solvetime);
         end
     end
diff --git a/templates/MPC_TS_SC_params.mat b/templates/MPC_TS_SC_params.mat
new file mode 100644
index 0000000..b8cc020
--- /dev/null
+++ b/templates/MPC_TS_SC_params.mat
diff --git a/templates/MPC_TS_SC_script.m b/templates/MPC_TS_SC_script.m
new file mode 100644
index 0000000..9499cf1
--- /dev/null
+++ b/templates/MPC_TS_SC_script.m
@@ -0,0 +1,14 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% Copyright (c) 2023, Amon Lahr, Simon Muntwiler, Antoine Leeman & Fabian Flürenbrock Institute for Dynamic Systems and Control, ETH Zurich.
+%
+% All rights reserved.
+%
+% Please see the LICENSE file that has been included as part of this package.
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+S=100000000*eye(6);
+v=100000000;
+matfile=struct("S",S,"v",v);
+save("MPC_TS_SC_params.mat","-struct","matfile")
+
+
diff --git a/templates/MPC_TUBE.m b/templates/MPC_TUBE.m
index bd62044..9a630c1 100644
--- a/templates/MPC_TUBE.m
+++ b/templates/MPC_TUBE.m
@@ -15,8 +15,48 @@ classdef MPC_TUBE
     methods
         function obj = MPC_TUBE(Q,R,N,H_N,h_N,H_tube,h_tube,K_tube,params)
             obj.K_tube = K_tube;
-
+    
             % YOUR CODE HERE
+            nx = params.model.nx;
+            nu = params.model.nu;
+
+            A = params.model.A;
+            B = params.model.B;
+
+            [~,P,~] = dlqr(A,B,Q,R);
+
+            V = sdpvar(repmat(nu,1,N),ones(1,N),'full');
+            Z = sdpvar(repmat(nx,1,N+1),ones(1,N+1),'full');
+
+            H_x = params.constraints.StateMatrix;
+            h_x = params.constraints.StateRHS;
+            H_u = params.constraints.InputMatrix;
+            h_u = params.constraints.InputRHS;
+
+            P_x_tightened = Polyhedron('A',H_x,'b',h_x).minus(Polyhedron('A',H_tube,'b',h_tube));
+            H_z = P_x_tightened.A;
+            h_z = P_x_tightened.b;
+
+            P_u_tightened = Polyhedron('A',H_u,'b',h_u).minus(K_tube*Polyhedron('A',H_tube,'b',h_tube));
+            H_v = P_u_tightened.A;
+            h_v = P_u_tightened.b;
+
+            X0 = sdpvar(nx,1,'full');
+            constraints = [H_tube*(X0-Z{1}) <= h_tube];
+            objective = 0;
+            for k = 1:N
+                constraints = [ ...
+                    constraints, ...
+                    Z{k+1} == A*Z{k} + B*V{k} , ...
+%                     H_z * Z{k} <= h_z, ...
+%                     H_v * V{k} <= h_v ...
+                    H_x * Z{k} <= h_x, ...
+                    H_u * V{k} <= h_u ...
+                ];
+                objective = objective + Z{k}'*Q*Z{k} + V{k}'*R*V{k};
+            end
+            constraints = [constraints, H_N * Z{end} <= h_N];
+            objective = objective + Z{end}'*P*Z{end};
 
             opts = sdpsettings('verbose',1,'solver','quadprog');
             obj.yalmip_optimizer = optimizer(constraints,objective,opts,X0,{V{1} Z{1} objective});
@@ -28,7 +68,10 @@ classdef MPC_TUBE
             [optimizer_out,errorcode] = obj.yalmip_optimizer(x);
             solvetime = toc;
             % YOUR CODE HERE
-            
+            [v0, z0, objective] = optimizer_out{:};
+            u = obj.K_tube*(x-z0)+v0;
+
+
             feasible = true;
             if (errorcode ~= 0)
                 feasible = false;
diff --git a/templates/MPC_TUBE_params.mat b/templates/MPC_TUBE_params.mat
new file mode 100644
index 0000000..9db7ef4
--- /dev/null
+++ b/templates/MPC_TUBE_params.mat
diff --git a/templates/MPC_TUBE_script.m b/templates/MPC_TUBE_script.m
new file mode 100644
index 0000000..a021cd0
--- /dev/null
+++ b/templates/MPC_TUBE_script.m
@@ -0,0 +1,15 @@
+p = [0.05, 0.1];
+params = generate_params();
+params = generate_params_z(params);
+K_tube = compute_tube_controller(p,params);
+[H_tube, h_tube, n_iter] = compute_minRPI(K_tube, params);
+params = compute_tightening(K_tube, H_tube, h_tube, params);
+
+Q = diag([300,0.1]);R = 1;N = 50;
+[H_N, h_N] = lqr_maxPI(Q, R, params);
+
+% MPC_TUBE_obj = MPC_TUBE(Q,R,N,H_N,h_N,H_tube,h_tube,K_tube,params);
+matfile = struct("p", p, "K_tube", K_tube, "H_tube", H_tube, "h_tube", h_tube,...
+                "H_N", H_N, "h_N", h_N, "params_z_tube", params);
+save("MPC_TUBE_params.mat", "-struct", "matfile");
+
diff --git a/templates/chebycenter.m b/templates/chebycenter.m
new file mode 100755
index 0000000..cbdce1c
--- /dev/null
+++ b/templates/chebycenter.m
@@ -0,0 +1,20 @@
+function [c,r] = chebycenter(A,b)
+%CHEBYCENTER Compute Chebyshev center of polytope Ax <= b.
+%  The Chebyshev center of a polytope is the center of the largest
+%  hypersphere enclosed by the polytope. 
+%  Requires optimization toolbox.
+
+[n,p] = size(A);
+an = sqrt(sum(A.^2,2));
+A1 = zeros(n,p+1);
+A1(:,1:p) = A;
+A1(:,p+1) = an;
+f = zeros(p+1,1);
+f(p+1) = -1;
+
+options = optimset;
+options = optimset(options,'Display', 'off');
+c = linprog(f,A1,b,[],[],[],[],[],options);
+r = c(p+1);
+c = c(1:p);
+end
diff --git a/templates/compute_minRPI.m b/templates/compute_minRPI.m
index 0b4ffdb..19df517 100644
--- a/templates/compute_minRPI.m
+++ b/templates/compute_minRPI.m
@@ -8,4 +8,32 @@
 
 function [H_tube,h_tube,n_iter] = compute_minRPI(K_tube,params)
     % YOUR CODE HERE
+    A = params.model.A+params.model.B*K_tube;
+    % A = params.model.A;
+%     display(A)
+    nx = params.model.nx;
+    omega = Polyhedron('A',[eye(nx);-eye(nx)],'b',zeros(1,2*nx)');
+    % display(omega.A)
+    % display(omega.b)
+    n_iter = 0;
+    Hw = params.constraints.DisturbanceMatrix;
+    hw = params.constraints.DisturbanceRHS;
+    W = Polyhedron('A',Hw,'b',hw); 
+    while true 
+        omega_last = copy(omega);
+        % display(n_iter)
+        % A_last = omega.A;
+        % b_last = omega.b;
+        display(n_iter);
+        display(omega.b)
+        omega = omega.plus(A^(n_iter)*W);
+        
+        if eq(omega.minHRep(),omega_last.minHRep())
+        % if eq(omega,omega_last)
+            break
+        end
+        n_iter = n_iter+1;
+    end
+    H_tube = omega.A;
+    h_tube = omega.b;
 end
\ No newline at end of file
diff --git a/templates/compute_tightening.m b/templates/compute_tightening.m
index 8919d12..7095f9c 100644
--- a/templates/compute_tightening.m
+++ b/templates/compute_tightening.m
@@ -8,4 +8,22 @@
 
 function params = compute_tightening(K_tube,H_tube,h_tube,params)  
 	% YOUR CODE HERE
+	Hx = params.constraints.StateMatrix;
+	hx = params.constraints.StateRHS;
+	
+	Hu = params.constraints.InputMatrix;
+	hu = params.constraints.InputRHS;
+	
+	Pxz = Polyhedron('A',Hx,'b',hx);
+	Puz = Polyhedron('A',Hu,'b',hu);
+	% Ptube = Polyhedron('A',H_tube,'b',h_tube);
+	
+	Px_tube = Pxz.minus(Polyhedron('A',H_tube,'b',h_tube));
+	params.constraints.StateMatrix = Px_tube.A;
+	params.constraints.StateRHS = Px_tube.b;
+    
+    Pu_tube = Puz.minus(K_tube*Polyhedron('A',H_tube,'b',h_tube))
+% 	Pu_tube = Puz.minus(Polyhedron('A',H_tube*K_tube,'b',h_tube));
+	params.constraints.InputMatrix = Pu_tube.A;
+	params.constraints.InputRHS = Pu_tube.b;
 end
\ No newline at end of file
diff --git a/templates/compute_tube_controller.m b/templates/compute_tube_controller.m
index adca95d..2c567f0 100644
--- a/templates/compute_tube_controller.m
+++ b/templates/compute_tube_controller.m
@@ -8,4 +8,9 @@
 
 function K_tube = compute_tube_controller(p,params)
 	% YOUR CODE HERE
+	Az = params.model.A;
+	Bz  =params.model.B;
+	% Bz = [params.model.B,eye(params.model.nx)];
+	K_tube = -place(Az,Bz,p);
+	% K_tube = K(1:params.model.nu,:);
 end
\ No newline at end of file
diff --git a/templates/cprnd.m b/templates/cprnd.m
new file mode 100755
index 0000000..4183ac3
--- /dev/null
+++ b/templates/cprnd.m
@@ -0,0 +1,296 @@
+function [X S] = cprnd(N,A,b,options)
+%CPRND Draw from the uniform distribution over a convex polytope.
+%   X = cprnd(N,A,b) Returns an N-by-P matrix of random vectors drawn
+%   from the uniform distribution over the interior of the polytope
+%   defined by Ax <= b. A is a M-by-P matrix of constraint equation
+%   coefficients. b is a M-by-1 vector of constraint equation
+%   constants.
+%    
+%   cprnd(N,A,b,options) allows various options to be specified.
+
+%   'method'    Specifies the algorithm. One of the strings 'gibbs',
+%               'hitandrun' (the default), and 'achr'. The default
+%               algorithm 'hitandrun' is a vanilla hit-and-run sampler
+%               [1]. 'gibbs' specifies a Gibbs sampler, 'achr' is the
+%               Adaptive Centering Hit-and-Run algorithm of [2]. 
+%
+%   'x0'        Vector x0 is a starting point which should be interior 
+%               to the polytope. If x0 is not supplied CPRND uses the
+%               Chebyshev center as the initial point.
+%    
+%   'isotropic' Perform an adaptive istropic transformation of the 
+%               polytope. The values 0, 1 and 2 respectively turn
+%               off the transformation, construct it during a runup
+%               period only, or continuously update the
+%               tranformation throughout sample production. The
+%               transformation makes sense only for the Gibbs and
+%               Hit-and-run samplers (ACHR is invariant under
+%               linear transformations).
+%   
+%   'discard'   Number of initial samples (post-runup) to discard.
+%
+%   'runup'     When the method is gibbs or hitandrun and the
+%               isotropic transformation is used, this is the
+%               number of initial iterations of the algorithm in
+%               the untransformed space. That is a sample of size
+%               runup is generated and its covariance used as the
+%               basis of a transformation. 
+%               When the method is achr runup is the number of
+%               conventional hitandrun iterations. See [2].
+%
+%   'ortho'     Zero/one flag. If turned on direction vectors for
+%               the hitandrun algorithm are generated in random
+%               orthonormal blocks rather than one by
+%               one. Experimental and of dubious utility.
+%   
+%   'quasi' Allows the user to specify a quasirandom number generator
+%               (such as 'halton' or 'sobol').  Experimental and of
+%               dubious utility.
+%
+%      
+
+%   By default CPRND employs the hit-and-run sampler which may
+%   exhibit marked serial correlation and very long convergence times.
+%
+% References
+% [1] Kroese, D.P. and Taimre, T. and Botev, Z.I., "Handbook of Monte
+% Carlo Methods" (2011), pp. 240-244.
+% [2] Kaufman, David E. and Smith, Robert L., "Direction Choice for
+% Accelerated Convergence in Hit-and-Run Sampling", Op. Res. 46,
+% pp. 84-95.
+%
+% Copyright (c) 2011-2012 Tim J. Benham, School of Mathematics and Physics,
+%                University of Queensland.
+
+    function y = stdize(z)
+        y = z/norm(z);
+    end
+
+    p = size(A,2);                         % dimension
+    m = size(A,1);                         % num constraint ineqs
+    x0 = [];
+    runup = [];                              % runup necessary to method
+    discard = [];                            % num initial pts discarded
+    quasi = 0;
+    method = 'achr';
+    orthogonal = 0;
+    isotropic = [];
+    
+    % gendir generates a random unit (direction) vector.
+    gendir = @() stdize(randn(p,1));
+
+    % Alternative function ogendir forces directions to come in
+    % orthogonal bunches.
+    Ucache = {};
+    function u = ogendir()
+        if length(Ucache) == 0
+            u = stdize(randn(p,1));
+            m = null(u');               % orthonormal basis for nullspace
+            Ucache = mat2cell(m',ones(1,p-1));
+        else
+            u = Ucache{end}';
+            Ucache(end) = [];
+        end
+    end
+    
+    % Check input arguments
+    
+    if m < p+1                             
+        % obv a prob here
+        error('cprnd:obvprob',['At least ',num2str(p+1),' inequalities ' ...
+                            'required']);
+    end
+
+    if nargin == 4
+        if isstruct(options)
+
+            if isfield(options,'method')
+                method = lower(options.method);
+                switch method
+                  case 'gibbs'
+                  case 'hitandrun'
+                  case 'achr'
+                  otherwise
+                    error('cprnd:badopt',...
+                          ['The method option takes only the ' ...
+                           'values "gibbs", "hitandrun", and "ACHR".']);
+                end
+            end
+            
+            if isfield(options,'isotropic')
+                % Controls application of isotropic transformation,
+                % which seems to help a lot.
+                isotropic = options.isotropic;
+            end
+
+            if isfield(options,'discard')
+                % num. of samples to discard
+                discard = options.discard;
+            end
+
+            if isfield(options,'quasi')
+                % Use quasi random numbers, which doesn't seem to
+                % help much.
+                quasi = options.quasi;
+                if quasi && ~ischar(quasi), quasi='halton'; end
+                if ~strcmp(quasi,'none')
+                    qstr = qrandstream(quasi,p,'Skip',1);
+                    gendir = @() stdize(norminv(qrand(qstr,1),0,1)');
+                end
+            end
+
+            if isfield(options,'x0')
+                % initial interior point
+                x0 = options.x0;
+            end
+
+            if isfield(options,'runup')
+                % number of points to generate before first output point
+                runup = options.runup;
+            end
+            
+            if isfield(options,'ortho')
+                % Generate direction vectors in orthogonal
+                % groups. Seems to help a little.
+                orthogonal = options.ortho;
+            end
+            
+        else
+            x0 = options;                   % legacy support
+        end
+    end
+
+    % Default and apply options
+    
+    if isempty(isotropic)
+        if ~strcmp(method,'achr')
+            isotropic = 2;
+        else
+            isotropic = 0;
+        end
+    end
+    
+    if orthogonal
+        gendir = @() ogendir();
+    end
+    
+    % Choose a starting point x0 if user did not provide one.
+    if isempty(x0)
+        x0 = chebycenter(A,b); % prob. if p==1?
+    end
+    
+    % Default the runup to something arbitrary.
+    if isempty(runup)
+        if strcmp(method,'achr')
+            runup = 10*(p+1);
+        elseif isotropic > 0
+            runup = 10*p*(p+1);
+        else 
+            runup = 0;
+        end
+    end
+
+    % Default the discard to something arbitrary
+    if isempty(discard)
+        if strcmp(method,'achr')
+            discard = 25*(p+1);
+        else
+            discard = runup;
+        end
+    end
+
+    X = zeros(N+runup+discard,p);
+
+    n = 0;                                  % num generated so far
+    x = x0;
+
+    % Initialize variables for keeping track of sample mean, covariance
+    % and isotropic transform.
+    M = zeros(p,1);                         % Incremental mean.
+    S2 = zeros(p,p);                        % Incremental sum of
+                                            % outer prodcts.
+    S = eye(p); T = eye(p); W = A;
+    
+    while n < N+runup+discard
+        y = x;
+        
+        % compute approximate stochastic transformation
+        if isotropic>0
+            if n == runup || (isotropic > 1 && n > runup)
+                T = chol(S,'lower');
+                W = A*T;
+            end
+            y = T\y;
+        end
+        
+        switch method
+            
+          case 'gibbs'
+            % choose p new components
+            for i = 1:p
+                % Find points where the line with the (p-1) components x_i
+                % fixed intersects the bounding polytope.
+                e = circshift(eye(p,1),i-1);
+                z = W*e;
+                c = (b - W*y)./z;
+                tmin = max(c(z<0)); tmax = min(c(z>0));
+                % choose a point on that line segment
+                y = y + (tmin+(tmax-tmin)*rand)*e;
+            end
+            
+          case 'hitandrun'
+            % choose a direction
+            u = gendir();
+            % determine intersections of x + ut with the polytope
+            z = W*u;
+            c = (b - W*y)./z;
+            tmin = max(c(z<0)); tmax = min(c(z>0));
+            % choose a point on that line segment
+            y = y + (tmin+(tmax-tmin)*rand)*u;
+            
+          case 'achr'
+            % test whether in runup or not
+            if n < runup
+                % same as hitandrun
+                u = gendir();
+            else
+                % choose a previous point at random
+                v = X(randi(n),:)';
+                % line sampling direction is from v to sample mean
+                u = (v-M)/norm(v-M);
+            end
+            % proceed as in hit and run
+            z = A*u;
+            c = (b - A*y)./z;
+            tmin = max(c(z<0)); tmax = min(c(z>0));
+            % Choose a random point on that line segment
+            y = y + (tmin+(tmax-tmin)*rand)*u;
+        end
+        
+        if isotropic>0
+            x = T*y;
+        else
+            x = y;
+        end
+        
+        X(n+1,:)=x';
+        n = n + 1;
+        
+        % Incremental mean and covariance updates
+        delta0 = x - M;       % delta new point wrt old mean
+        M = M + delta0/n;     % sample mean
+        delta1 = x - M;       % delta new point wrt new mean
+        if n > 1
+            S2 = S2 + (n-1)/(n*n)*delta0*delta0'...
+                 + delta1*delta1';
+            S0 = S;
+            S = S2/(n-1);           % sample covariance
+        else 
+            S = eye(p);
+        end
+        
+    end
+        
+    X = X((discard+runup+1):(N+discard+runup),:); 
+
+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_disturbances.m b/templates/generate_disturbances.m
index 570a4c9..f08cdb5 100644
--- a/templates/generate_disturbances.m
+++ b/templates/generate_disturbances.m
@@ -8,4 +8,10 @@
 
 function Wt = generate_disturbances(params)
     % YOUR CODE HERE
+    % params_z = generate_params_z(params);
+    Hw = params.constraints.DisturbanceMatrix;
+    hw = params.constraints.DisturbanceRHS;
+    % Pw = Polyhedron('A', Hw, 'b', hw);
+    N = params.model.HorizonLength;
+    Wt = cprnd(N,Hw,hw)';
 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/lqr_maxPI.m b/templates/lqr_maxPI.m
index efa335a..99b5b22 100644
--- a/templates/lqr_maxPI.m
+++ b/templates/lqr_maxPI.m
@@ -6,7 +6,40 @@
 % Please see the LICENSE file that has been included as part of this package.
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-function [H, h] = lqr_maxPI(Q,R,params)
-	% YOUR CODE HERE
+
+function [H, h] = lqr_maxPI(Q, R, params)
+    % Define the linear system
+    A = params.model.A;
+    B = params.model.B;
+    nx=params.model.nx;
+    nu=params.model.nu;
+    
+    s_max= params.constraints.MaxAbsPositionXZ;
+    y_max= params.constraints.MaxAbsPositionY;
+    u_max = params.constraints.MaxAbsThrust;
+    
+    K = -dlqr(A, B, Q, R);
+    systemLQR = LTISystem('A', A+B*K);
+%     absxmax = ones(nx,1)*s_max;
+%     absumax = ones(nu,1)*u_max;
+%     Xp = Polyhedron('A',[eye(nx); -eye(nx); K; -K], 'b', [absxmax;absxmax; absumax;absumax]);
+    Hx=params.constraints.StateMatrix;
+    hx=params.constraints.StateRHS;
+    Hu=params.constraints.InputMatrix;
+    hu=params.constraints.InputRHS;
+    Xp = Polyhedron('A',[Hx;Hu*K], 'b', [hx;hu]);
+    
+    figure(1);
+    systemLQR.x.with('setConstraint');
+    systemLQR.x.setConstraint = Xp;
+    InvSetLQR = systemLQR.invariantSet();
+%     InvSetLQR.plot(), alpha(0.25), title('Invariant Set for Triple Integrator under LQR Control'), xlabel('x_1'), ylabel('x_2'), zlabel('x_3');
+
+    H=InvSetLQR.A;
+    h=InvSetLQR.b;
+
+
 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/simulate_uncertain.m b/templates/simulate_uncertain.m
index dc9df26..f74f7af 100644
--- a/templates/simulate_uncertain.m
+++ b/templates/simulate_uncertain.m
@@ -8,4 +8,17 @@
 
 function [Xt,Ut,ctrl_info] = simulate_uncertain(x0, ctrl, Wt, params)
 	% YOUR CODE HERE
+	Xt = [x0];
+    Ut = [];
+    x = x0
+    ctrl_info = [];
+    % obj = LQR();
+    for i = 1:params.model.HorizonLength
+        [u, ctrl_info_] = ctrl.eval(x);
+        Ut = [Ut u];
+        ctrl_info = [ctrl_info ctrl_info_];
+        x = params.model.A*x + params.model.B*u + Wt(:,i);
+        Xt = [Xt x];
+    end
+    % plot_trajectory(Xt, Ut, ctrl_info, params);
 end
\ No newline at end of file
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
 
diff --git a/templates/traj_cost.m b/templates/traj_cost.m
index 7e0b443..57566c9 100644
--- a/templates/traj_cost.m
+++ b/templates/traj_cost.m
@@ -8,4 +8,10 @@
 
 function J_Nt = traj_cost(Xt,Ut,Q,R)
     % YOUR CODE HERE
+    J_Nt=0;
+    for i=1:length(Xt(1,:))-1
+        a= Xt(:,i)'*Q*Xt(:,i);
+        b= Ut(:,i)'*R*Ut(:,i);
+        J_Nt=J_Nt+a+b;
+    end
 end
\ No newline at end of file