ADD: raw 30

2 files changed, 148 insertions, 1 deletions

diff --git a/Installation.m b/Installation.m
new file mode 100644
index 0000000..97981dd
--- /dev/null
+++ b/Installation.m
@@ -0,0 +1,108 @@
+%% installation of tbxmanager with all submodels required for MPT
+%
+
+clc;
+disp('----------------------------------------------');
+disp('Installation of MPT using the Toolbox manager.');
+disp('----------------------------------------------');
+disp(' ');
+fprintf(['Choose the installation directory where to install the Toolbox manager.\n',...
+      'A new folder "tbxmanager" is going to be created in the specified location.\n',...
+      'If you do not specify the folder, the Toolbox manager will be installed in the current directory.\n']);
+
+% get the installation folder
+default_dir = pwd;
+c = uigetdir(pwd);
+if isequal(c,0);
+    fprintf(['No directory has been provided.\n',... 
+        'Installing the toolbox manager in the current directory "%s"?\n'],default_dir);
+    c = default_dir;
+end
+ 
+% create a new directory in that folder
+d = [c,filesep,'tbxmanager'];
+if isequal(exist(d,'dir'),7)
+    error('The installation directory "%s" already exists.\nPlease, remove or rename the folder or change the installation path.',d);
+end
+disp('Creating the directory "tbxmanager".');
+out = mkdir(d);
+if ~out
+    error(['An error appear when trying to create the folder "%s".\n',...
+          'Please, install the Toolbox manager manually.'],c); 
+end
+
+% enter that directory
+cd(d);
+
+% remove MPT2 or YALMIP
+disp(' ');
+disp('Removing toolboxes that may conflict with MPT from the Matlab path.');
+rmpath(genpath(fileparts(which('mpt_init'))));
+rmpath(genpath(fileparts(which('yalmipdemo'))));
+
+
+% download the tbxmanager
+disp(' ');
+disp('Downloading the Toolbox manager from the internet.');
+[f, c] = urlwrite('http://www.tbxmanager.com/tbxmanager.m', 'tbxmanager.m');
+rehash;
+
+if isequal(c,0)
+    error('Could not download the Toolbox manager from the internet. The installation cannot continue.');
+end
+
+% install all required modules
+tbxmanager install mpt mptdoc cddmex fourier glpkmex hysdel lcp sedumi yalmip 
+
+% create the initialization file to set the path 
+disp(' ');
+disp('Creating the initialization file "startup.m".');
+p = which('startup.m');
+if isempty(p)
+    p = [d,filesep,'startup.m'];
+end
+fid = fopen(p,'a');
+if isequal(fid,-1)
+    error(['Could not modify the initialization file "startup.m".',...
+           'Edit this file in the folder "%s" manually and insert there the line:  tbxmanager restorepath.'],p);
+end
+fprintf(fid,'tbxmanager restorepath\n');
+fclose(fid);
+disp('File has been created.');
+
+% get back to the original directory
+cd(default_dir);
+
+% add path to tbxmanager
+disp(' ');
+disp('Adding path to Matlab.');
+addpath(d);
+
+% save path for future
+disp(' ');
+disp('Saving path for future sessions.');
+status = savepath;
+
+if status
+    fprintf('Could not save the path to a default location,\nplease provide a location where you want to save the path.');
+    cn = uigetdir(pwd);
+    if isequal(cn,0)
+        disp(' ');
+        fprintf('No directory specified, saving the path to the current directory "%s".\n\n',default_dir);
+        cn = default_dir;
+    end
+    sn = savepath([cn,filesep,'pathdef.m']);
+    if sn
+        error(['Could not save the path automatically.\n',...
+            'Please, open the "Set Path" button in the Matlab menu and save the path manually to some location.']);
+    end
+end
+
+disp(' ');
+disp('Installation finished.');
+disp('Next time you start Matlab the toolboxes will be automatically initialized.');
+
+% initialize MPT
+disp(' ');
+disp('Initializing the MPT.')
+mpt_init;
diff --git a/templates/MPC_TUBE.m b/templates/MPC_TUBE.m
index bd62044..83b056b 100644
--- a/templates/MPC_TUBE.m
+++ b/templates/MPC_TUBE.m
@@ -17,6 +17,44 @@ classdef MPC_TUBE
             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*(Z{1}-X0) <= 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 ...
+                ];
+                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 +66,8 @@ classdef MPC_TUBE
             [optimizer_out,errorcode] = obj.yalmip_optimizer(x);
             solvetime = toc;
             % YOUR CODE HERE
-            
+            [u, objective] = optimizer_out{:};
+
             feasible = true;
             if (errorcode ~= 0)
                 feasible = false;