ADD: pass system modeling test

6 files changed, 167 insertions, 0 deletions

diff --git a/install_mpt3.m b/install_mpt3.m
new file mode 100644
index 0000000..97981dd
--- /dev/null
+++ b/install_mpt3.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/generate_constraints.m b/templates/generate_constraints.m
index 892b706..f87ac26 100644
--- a/templates/generate_constraints.m
+++ b/templates/generate_constraints.m
@@ -8,4 +8,12 @@
 
 function [H_u, h_u, H_x, h_x] = generate_constraints(params)
     % YOUR CODE HERE
+    u_max = params.constraints.MaxAbsThrust;
+    h_u = u_max * ones(6,1);
+    H_u = [eye(3);-eye(3)];
+    
+    s_max = params.constraints.MaxAbsPositionXZ;
+    y_max = params.constraints.MaxAbsPositionY;
+    h_x = [s_max;y_max;s_max;s_max;y_max;s_max];
+    H_x = [eye(3);-eye(3)];
 end
\ No newline at end of file
diff --git a/templates/generate_params.m b/templates/generate_params.m
index ef51366..e2e6024 100644
--- a/templates/generate_params.m
+++ b/templates/generate_params.m
@@ -41,5 +41,33 @@ params.exercise = struct( ...
 );
     
 % YOUR CODE HERE
+[Ac, Bc] = generate_system_cont(params);
+[A_tilta, B_tilta] = generate_system(Ac, Bc, params);
+[A, B] = generate_system_scaled(A_tilta, B_tilta, params);
+[Hu, hu, Hx, hx] = generate_constraints(params);
+
+params.model.A = A;
+params.model.B = B;
+
+params.constraints.InputMatrix = Hu;
+params.constraints.InputRHS = hu;
+params.constraints.StateMatrix = Hx;
+params.constraints.StateRHS = hx;
+
+% new_model_struct = struct(...
+%             'A', A, ...
+%             'B', B ...
+% );
+
+% new_constr_struct = struct(...
+%             'InputMatrix', Hu,...
+%             'InputRHS', hu,...
+%             'StateMatrix', Hx,...
+%             'StateRHS', hx ...
+% )
+
+
+% params.model = [params.model new_model_struct];
+% params.constraints = [params.constraints, new_constr_struct];
 
 end
diff --git a/templates/generate_system.m b/templates/generate_system.m
index 9deb347..c4dadc2 100644
--- a/templates/generate_system.m
+++ b/templates/generate_system.m
@@ -8,4 +8,12 @@
 
 function [A, B] = generate_system(Ac, Bc, params)
     % YOUR CODE HERE
+    % Ts = 600;
+    sysc = ss(Ac, Bc, [], []);
+    Ts = params.model.TimeStep;
+    sysd = c2d(sysc,Ts);
+    % A = eye(params.model.nx) + Ac * Ts;
+    % B = Bc * Ts;
+    A = sysd.A;
+    B = sysd.B;
 end
\ No newline at end of file
diff --git a/templates/generate_system_cont.m b/templates/generate_system_cont.m
index 2d3ee79..770fd8e 100644
--- a/templates/generate_system_cont.m
+++ b/templates/generate_system_cont.m
@@ -8,4 +8,15 @@
 
 function [Ac, Bc] = generate_system_cont(params)
     % YOUR CODE HERE
+    miu = params.model.GravitationalParameter;
+    r = params.model.TargetRadius;
+    omega_n = sqrt(miu/(r^3));
+    m = params.model.Mass;
+    Ac = [0,0,0,1,0,0;
+          0,0,0,0,1,0;
+          0,0,0,0,0,1;
+          3*omega_n^2,0,0,0,2*omega_n,0;
+          0,0,0,-2*omega_n,0,0;
+          0,0,-omega_n^2,0,0,0];
+    Bc = [zeros(3,3);1/m*eye(3)];
 end
\ No newline at end of file
diff --git a/templates/generate_system_scaled.m b/templates/generate_system_scaled.m
index eac8db8..38714d2 100644
--- a/templates/generate_system_scaled.m
+++ b/templates/generate_system_scaled.m
@@ -8,4 +8,8 @@
 
 function [A,B] = generate_system_scaled(At,Bt,params)
     % YOUR CODE HERE
+    % V = diag([1e-6, 1e-6, 1e-6, 1e-3, 1e-3, 1e-3]);
+    V = params.model.ScalingMatrix;
+    A = V*At*inv(V);
+    B = V*Bt;
 end
\ No newline at end of file