summaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
authorYanzhenXiangRobotics <xyz000327@gmail.com>2023-04-25 16:42:53 +0200
committerYanzhenXiangRobotics <xyz000327@gmail.com>2023-04-25 16:42:53 +0200
commitbbf8c17a58001bfe15948e3b2a31ca2a1a95f928 (patch)
tree3b66cfa5a8e229b94775c8dd97587569dde61793
parentfirst commit (diff)
downloadmpc_pe-bbf8c17a58001bfe15948e3b2a31ca2a1a95f928.tar.gz
mpc_pe-bbf8c17a58001bfe15948e3b2a31ca2a1a95f928.zip
ADD: init proj
Diffstat (limited to '')
-rw-r--r--project2023.pdfbin0 -> 360222 bytes
-rw-r--r--templates/.MATLABDriveTag1
-rw-r--r--templates/LICENSE.txt7
-rw-r--r--templates/LQR.m27
-rw-r--r--templates/MPC.m45
-rw-r--r--templates/MPC_TE.m36
-rw-r--r--templates/MPC_TE_forces.m38
-rw-r--r--templates/MPC_TS.m36
-rw-r--r--templates/MPC_TS_SC.m36
-rw-r--r--templates/MPC_TUBE.m40
-rw-r--r--templates/compute_minRPI.m11
-rw-r--r--templates/compute_tightening.m11
-rw-r--r--templates/compute_tube_controller.m11
-rw-r--r--templates/generate_constraints.m11
-rw-r--r--templates/generate_disturbances.m11
-rw-r--r--templates/generate_params.m45
-rw-r--r--templates/generate_params_z.m55
-rw-r--r--templates/generate_system.m11
-rw-r--r--templates/generate_system_cont.m11
-rw-r--r--templates/generate_system_scaled.m11
-rw-r--r--templates/lqr_maxPI.m12
-rw-r--r--templates/lqr_tuning.m11
-rw-r--r--templates/plot_trajectory.m126
-rw-r--r--templates/plot_trajectory_z.m78
-rw-r--r--templates/simulate.m14
-rw-r--r--templates/simulate_uncertain.m11
-rw-r--r--templates/traj_constraints.m12
-rw-r--r--templates/traj_cost.m11
-rw-r--r--testing/.MATLABDriveTag1
-rw-r--r--testing/TestFunction.pbin0 -> 3855 bytes
-rw-r--r--testing/TestFunction_LQR.pbin0 -> 970 bytes
-rw-r--r--testing/TestFunction_MPC.pbin0 -> 1717 bytes
-rw-r--r--testing/TestFunction_MPC_TE.pbin0 -> 1659 bytes
-rw-r--r--testing/TestFunction_MPC_TE_forces.pbin0 -> 1670 bytes
-rw-r--r--testing/TestFunction_MPC_TS.pbin0 -> 1761 bytes
-rw-r--r--testing/TestFunction_MPC_TS_SC.pbin0 -> 2187 bytes
-rw-r--r--testing/TestFunction_MPC_TS_SC_script.pbin0 -> 1179 bytes
-rw-r--r--testing/TestFunction_MPC_TUBE.pbin0 -> 1857 bytes
-rw-r--r--testing/TestFunction_MPC_TUBE_script.pbin0 -> 1033 bytes
-rw-r--r--testing/TestFunction_compute_minRPI.pbin0 -> 593 bytes
-rw-r--r--testing/TestFunction_compute_tightening.pbin0 -> 1000 bytes
-rw-r--r--testing/TestFunction_compute_tube_controller.pbin0 -> 647 bytes
-rw-r--r--testing/TestFunction_generate_constraints.pbin0 -> 763 bytes
-rw-r--r--testing/TestFunction_generate_disturbances.pbin0 -> 864 bytes
-rw-r--r--testing/TestFunction_generate_params.pbin0 -> 1247 bytes
-rw-r--r--testing/TestFunction_generate_system.pbin0 -> 840 bytes
-rw-r--r--testing/TestFunction_generate_system_cont.pbin0 -> 852 bytes
-rw-r--r--testing/TestFunction_generate_system_scaled.pbin0 -> 886 bytes
-rw-r--r--testing/TestFunction_lqr_maxPI.pbin0 -> 785 bytes
-rw-r--r--testing/TestFunction_lqr_tuning.pbin0 -> 1529 bytes
-rw-r--r--testing/TestFunction_lqr_tuning_script.pbin0 -> 906 bytes
-rw-r--r--testing/TestFunction_simulate.pbin0 -> 983 bytes
-rw-r--r--testing/TestFunction_simulate_uncertain.pbin0 -> 1251 bytes
-rw-r--r--testing/TestFunction_traj_constraints.pbin0 -> 1405 bytes
-rw-r--r--testing/TestFunction_traj_cost.pbin0 -> 694 bytes
-rw-r--r--testing/linearController.pbin0 -> 183 bytes
-rw-r--r--testing/run_tests.pbin0 -> 3072 bytes
57 files changed, 730 insertions, 0 deletions
diff --git a/project2023.pdf b/project2023.pdf
new file mode 100644
index 0000000..bde0b0a
--- /dev/null
+++ b/project2023.pdf
Binary files differ
diff --git a/templates/.MATLABDriveTag b/templates/.MATLABDriveTag
new file mode 100644
index 0000000..7aad4f3
--- /dev/null
+++ b/templates/.MATLABDriveTag
@@ -0,0 +1 @@
+f7c7757b-ada0-4bff-af70-1240edb65e6d \ No newline at end of file
diff --git a/templates/LICENSE.txt b/templates/LICENSE.txt
new file mode 100644
index 0000000..685f12c
--- /dev/null
+++ b/templates/LICENSE.txt
@@ -0,0 +1,7 @@
+Copyright (c) 2023, Amon Lahr, Simon Muntwiler, Antoine Leeman & Fabian Flürenbrock Institute for Dynamic Systems and Control, ETH Zurich.
+
+All rights reserved.
+
+The files are provided for educational purposes only and should be distributed only to the attendees of the class "151-0660-00L Model Predictive Control FS2023".
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
diff --git a/templates/LQR.m b/templates/LQR.m
new file mode 100644
index 0000000..2a79da9
--- /dev/null
+++ b/templates/LQR.m
@@ -0,0 +1,27 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% 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.
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+classdef LQR
+ properties
+ K
+ end
+
+ methods
+ %constructor
+ function obj = LQR(Q,R,params)
+ % YOUR CODE HERE
+ % obj.K = ... (save feedback matrix for use in eval function)
+ end
+
+ function [u, ctrl_info] = eval(obj,x)
+ % YOUR CODE HERE
+ % u = ...
+ ctrl_info = struct('ctrl_feas',true);
+ end
+ end
+end \ No newline at end of file
diff --git a/templates/MPC.m b/templates/MPC.m
new file mode 100644
index 0000000..3e9d2f1
--- /dev/null
+++ b/templates/MPC.m
@@ -0,0 +1,45 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% 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.
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+classdef MPC
+ properties
+ yalmip_optimizer
+ end
+
+ methods
+ function obj = MPC(Q,R,N,params)
+ nu = params.model.nu;
+ nx = params.model.nx;
+
+ % define optimization variables
+ U = sdpvar(repmat(nu,1,N),ones(1,N),'full');
+ X0 = sdpvar(nx,1,'full');
+
+ % YOUR CODE HERE
+
+ 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.
+ tic;
+ [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
diff --git a/templates/MPC_TE.m b/templates/MPC_TE.m
new file mode 100644
index 0000000..e0b55d2
--- /dev/null
+++ b/templates/MPC_TE.m
@@ -0,0 +1,36 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% 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.
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+classdef MPC_TE
+ properties
+ yalmip_optimizer
+ end
+
+ methods
+ 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});
+ end
+
+ function [u, ctrl_info] = eval(obj,x)
+ %% evaluate control action by solving MPC problem, e.g.
+ tic;
+ [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
diff --git a/templates/MPC_TE_forces.m b/templates/MPC_TE_forces.m
new file mode 100644
index 0000000..5625f32
--- /dev/null
+++ b/templates/MPC_TE_forces.m
@@ -0,0 +1,38 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% 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.
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+classdef MPC_TE_forces
+ properties
+ forces_optimizer
+ end
+
+ methods
+ function obj = MPC_TE_forces(Q,R,N,params)
+ % YOUR CODE HERE
+ opts = getOptions('forcesSolver');
+ opts.printlevel = 0;
+ obj.forces_optimizer = % YOUR CODE HERE
+ end
+
+ function [u, ctrl_info] = eval(obj,x)
+ %% evaluate control action by solving MPC problem, e.g.
+ [optimizer_out,errorcode,info] = obj.forces_optimizer(x);
+ u = optimizer_out;
+ objective = info.pobj;
+ solvetime = info.solvetime;
+
+ feasible = true;
+ if any(errorcode ~= 1)
+ feasible = false;
+ warning('MPC infeasible');
+ end
+
+ ctrl_info = struct('ctrl_feas',feasible,'objective',objective,'solvetime',solvetime);
+ end
+ end
+end \ No newline at end of file
diff --git a/templates/MPC_TS.m b/templates/MPC_TS.m
new file mode 100644
index 0000000..05f92ff
--- /dev/null
+++ b/templates/MPC_TS.m
@@ -0,0 +1,36 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% 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.
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+classdef MPC_TS
+ properties
+ yalmip_optimizer
+ end
+
+ methods
+ 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
+
+ function [u, ctrl_info] = eval(obj,x)
+ %% evaluate control action by solving MPC problem, e.g.
+ tic;
+ [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
diff --git a/templates/MPC_TS_SC.m b/templates/MPC_TS_SC.m
new file mode 100644
index 0000000..0e64767
--- /dev/null
+++ b/templates/MPC_TS_SC.m
@@ -0,0 +1,36 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% 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.
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+classdef MPC_TS_SC
+ properties
+ yalmip_optimizer
+ end
+
+ methods
+ function obj = MPC_TS_SC(Q,R,N,H,h,S,v,params)
+ % YOUR CODE HERE
+ 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.
+ tic;
+ [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
diff --git a/templates/MPC_TUBE.m b/templates/MPC_TUBE.m
new file mode 100644
index 0000000..bd62044
--- /dev/null
+++ b/templates/MPC_TUBE.m
@@ -0,0 +1,40 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% 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.
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+classdef MPC_TUBE
+ properties
+ yalmip_optimizer
+ K_tube
+ end
+
+ 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
+
+ opts = sdpsettings('verbose',1,'solver','quadprog');
+ obj.yalmip_optimizer = optimizer(constraints,objective,opts,X0,{V{1} Z{1} objective});
+ end
+
+ function [u, ctrl_info] = eval(obj,x)
+ %% evaluate control action by solving MPC problem, e.g.
+ tic;
+ [optimizer_out,errorcode] = obj.yalmip_optimizer(x);
+ solvetime = toc;
+ % YOUR CODE HERE
+
+ 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
diff --git a/templates/compute_minRPI.m b/templates/compute_minRPI.m
new file mode 100644
index 0000000..0b4ffdb
--- /dev/null
+++ b/templates/compute_minRPI.m
@@ -0,0 +1,11 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% 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.
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+function [H_tube,h_tube,n_iter] = compute_minRPI(K_tube,params)
+ % YOUR CODE HERE
+end \ No newline at end of file
diff --git a/templates/compute_tightening.m b/templates/compute_tightening.m
new file mode 100644
index 0000000..8919d12
--- /dev/null
+++ b/templates/compute_tightening.m
@@ -0,0 +1,11 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% 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.
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+function params = compute_tightening(K_tube,H_tube,h_tube,params)
+ % YOUR CODE HERE
+end \ No newline at end of file
diff --git a/templates/compute_tube_controller.m b/templates/compute_tube_controller.m
new file mode 100644
index 0000000..adca95d
--- /dev/null
+++ b/templates/compute_tube_controller.m
@@ -0,0 +1,11 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% 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.
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+function K_tube = compute_tube_controller(p,params)
+ % YOUR CODE HERE
+end \ No newline at end of file
diff --git a/templates/generate_constraints.m b/templates/generate_constraints.m
new file mode 100644
index 0000000..892b706
--- /dev/null
+++ b/templates/generate_constraints.m
@@ -0,0 +1,11 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% 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.
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+function [H_u, h_u, H_x, h_x] = generate_constraints(params)
+ % YOUR CODE HERE
+end \ No newline at end of file
diff --git a/templates/generate_disturbances.m b/templates/generate_disturbances.m
new file mode 100644
index 0000000..570a4c9
--- /dev/null
+++ b/templates/generate_disturbances.m
@@ -0,0 +1,11 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% 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.
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+function Wt = generate_disturbances(params)
+ % YOUR CODE HERE
+end \ No newline at end of file
diff --git a/templates/generate_params.m b/templates/generate_params.m
new file mode 100644
index 0000000..ef51366
--- /dev/null
+++ b/templates/generate_params.m
@@ -0,0 +1,45 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% 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.
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+function [params] = generate_params()
+params = struct();
+
+Tf = 60*60*24 * 2; % = 2 days
+dt = 60 * 10; % = 10 minutes
+
+% model
+params.model = struct(...
+ 'nx', 6, ...
+ 'nu', 3, ...
+ 'Mass', 300, ...
+ 'GravitationalParameter', 3.986e14, ...
+ 'ScalingMatrix', [1e-6*eye(3), zeros(3); zeros(3), 1e-3*eye(3)], ...
+ 'TargetRadius', 7000e3, ...
+ 'TimeStep', dt, ...
+ 'HorizonLength', ceil(Tf / dt), ...
+ 'InitialConditionA', [-15e-3; -400e-3; 24.4e-3; 0; 0.0081; 0], ...
+ 'InitialConditionB', [-20e-3; 400e-3; 24.4e-3; 0; 0.0108; 0], ...
+ 'InitialConditionC', [0.02; 0.01; -0.005; 0; 0; 0] ...
+);
+
+% constraints
+params.constraints = struct(...
+ 'MaxAbsPositionXZ', 0.1, ...
+ 'MaxAbsPositionY', 1, ...
+ 'MaxAbsThrust', 1, ...
+ 'MaxFinalPosDiff' , 3e-4, ...
+ 'MaxFinalVelDiff', 1e-3 ...
+);
+
+params.exercise = struct( ...
+ 'QdiagOptA', [94.0; 0.1579; 300; 0.01; 0.10; 0.10] ...
+);
+
+% YOUR CODE HERE
+
+end
diff --git a/templates/generate_params_z.m b/templates/generate_params_z.m
new file mode 100644
index 0000000..a7cfaa9
--- /dev/null
+++ b/templates/generate_params_z.m
@@ -0,0 +1,55 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% 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.
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+function [params_z] = generate_params_z(params)
+% initialize params_z
+params_z = params;
+
+% add initial condition of z-subsystem
+params_z.model = rmfield(params_z.model,{'InitialConditionA', ...
+ 'InitialConditionB', ...
+ 'InitialConditionC'});
+params_z.model.InitialConditionA_z = [-0.07;0.04]; % TODO: tune
+
+% define projection matrices
+T_z_x = [0 0 1 0 0 0;
+ 0 0 0 0 0 1];
+T_z_u = [0 0 1];
+
+% projected system dynamics
+params_z.model.A = T_z_x*params.model.A*T_z_x';
+params_z.model.B = T_z_x*params.model.B*T_z_u';
+params_z.model.nx = 2;
+params_z.model.nu = 1;
+
+% state constraints
+Hx = params.constraints.StateMatrix * T_z_x';
+hx = params.constraints.StateRHS;
+X = Polyhedron('A',Hx,'b',hx);
+X.minHRep();
+
+params_z.constraints.StateMatrix = X.A;
+params_z.constraints.StateRHS = X.b;
+
+% input constraints
+Hu = params.constraints.InputMatrix * T_z_u';
+hu = params.constraints.InputRHS;
+U = Polyhedron('A',Hu,'b',hu);
+
+params_z.constraints.InputMatrix = U.A;
+params_z.constraints.InputRHS = U.b;
+
+% disturbance constraints
+max_disturbance = 1e-4; % TODO: chose
+params_z.constraints.MaxAbsDisturbance = max_disturbance;
+H_w = kron(eye(params_z.model.nx),[1;-1]);
+h_w = ones(2*params_z.model.nx, 1) * max_disturbance;
+
+params_z.constraints.DisturbanceMatrix = H_w;
+params_z.constraints.DisturbanceRHS = h_w;
+end
diff --git a/templates/generate_system.m b/templates/generate_system.m
new file mode 100644
index 0000000..9deb347
--- /dev/null
+++ b/templates/generate_system.m
@@ -0,0 +1,11 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% 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.
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+function [A, B] = generate_system(Ac, Bc, params)
+ % YOUR CODE HERE
+end \ No newline at end of file
diff --git a/templates/generate_system_cont.m b/templates/generate_system_cont.m
new file mode 100644
index 0000000..2d3ee79
--- /dev/null
+++ b/templates/generate_system_cont.m
@@ -0,0 +1,11 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% 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.
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+function [Ac, Bc] = generate_system_cont(params)
+ % YOUR CODE HERE
+end \ No newline at end of file
diff --git a/templates/generate_system_scaled.m b/templates/generate_system_scaled.m
new file mode 100644
index 0000000..eac8db8
--- /dev/null
+++ b/templates/generate_system_scaled.m
@@ -0,0 +1,11 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% 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.
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+function [A,B] = generate_system_scaled(At,Bt,params)
+ % YOUR CODE HERE
+end \ No newline at end of file
diff --git a/templates/lqr_maxPI.m b/templates/lqr_maxPI.m
new file mode 100644
index 0000000..efa335a
--- /dev/null
+++ b/templates/lqr_maxPI.m
@@ -0,0 +1,12 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% 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.
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+function [H, h] = lqr_maxPI(Q,R,params)
+ % YOUR CODE HERE
+end
+
diff --git a/templates/lqr_tuning.m b/templates/lqr_tuning.m
new file mode 100644
index 0000000..4a26158
--- /dev/null
+++ b/templates/lqr_tuning.m
@@ -0,0 +1,11 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% 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.
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+function [tuning_struct, i_opt] = lqr_tuning(x0,Q,params)
+ % YOUR CODE HERE
+end \ No newline at end of file
diff --git a/templates/plot_trajectory.m b/templates/plot_trajectory.m
new file mode 100644
index 0000000..9bcb696
--- /dev/null
+++ b/templates/plot_trajectory.m
@@ -0,0 +1,126 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% 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.
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+function [fig_time,axes_time,fig_pos,axes_pos] = plot_trajectory(x,u,ctrl_info,params)
+ fig_time = figure;
+
+ % check if input is 3-dimensional
+ n_traj = size(x,3);
+ assert(size(u,3) == n_traj);
+ assert(size(ctrl_info,3) == n_traj);
+
+ nx = params.model.nx;
+ assert(nx == size(x,1));
+
+ t = 0:params.model.TimeStep:params.model.TimeStep*params.model.HorizonLength;
+
+ % plot
+ axes_time = cell(5,1);
+ axes_time{1} = subplot(5,1,1);
+ hold on;
+
+ for i = 1:n_traj
+ plot(axes_time{1},t,x(1,:,i),'DisplayName',sprintf('x_%d',i));
+ plot(axes_time{1},t,x(3,:,i),'DisplayName',sprintf('z_%d',i));
+ end
+ legend('Location','EastOutside')
+
+ % max position
+ x_max = params.constraints.MaxAbsPositionXZ;
+ plot(axes_time{1}, [t(1); t(end)],[x_max; x_max],'k--','HandleVisibility','off');
+ plot(axes_time{1}, [t(1); t(end)],[-x_max; -x_max],'k--','HandleVisibility','off');
+ ylabel('Position [Mm]')
+
+ axes_time{2} = subplot(5,1,2);
+ hold on;
+ for i = 1:n_traj
+ plot(axes_time{2},t,x(2,:,i),'DisplayName',sprintf('y_%d',i));
+ end
+
+ % max position
+ y_max = params.constraints.MaxAbsPositionY;
+ plot(axes_time{2},[t(1); t(end)],[y_max; y_max],'k--','HandleVisibility','off');
+ plot(axes_time{2},[t(1); t(end)],[-y_max; -y_max],'k--','HandleVisibility','off');
+
+ legend('Location','EastOutside')
+ ylabel('Position [Mm]')
+
+ axes_time{3} = subplot(5,1,3);
+ hold on;
+ for i = 1:n_traj
+ plot(axes_time{3},t,x(4,:,i),'DisplayName',sprintf('v_{x%d}',i));
+ plot(axes_time{3},t,x(5,:,i),'DisplayName',sprintf('v_{y%d}',i));
+ plot(axes_time{3},t,x(6,:,i),'DisplayName',sprintf('v_{z%d}',i));
+ end
+
+ legend('Location','EastOutside')
+ ylabel('Velocity [km/s]')
+
+ axes_time{4} = subplot(5,1,4);
+ hold on;
+ % append one input value for plotting
+ u(:,length(t),:) = u(:,length(t)-1,:);
+ for i = 1:n_traj
+ stairs(axes_time{4},t,u(1,:,i)','DisplayName',sprintf('u_{x%d}',i));
+ stairs(axes_time{4},t,u(2,:,i)','DisplayName',sprintf('u_{y%d}',i));
+ stairs(axes_time{4},t,u(3,:,i)','DisplayName',sprintf('u_{z%d}',i));
+ end
+
+ % max thrust
+ thrust_max = params.constraints.MaxAbsThrust;
+ plot([t(1); t(end)],[thrust_max; thrust_max],'k--','HandleVisibility','off');
+ plot([t(1); t(end)],[-thrust_max; -thrust_max],'k--','HandleVisibility','off');
+ legend('Location','EastOutside')
+ ylabel('Thrust [N]')
+
+ % feasibility
+ axes_time{5} = subplot(5,1,5);
+ hold on;
+ for i = 1:n_traj
+ % append one input value for plotting
+ ctrl_feas = [ctrl_info(:,:,i).ctrl_feas];
+ ctrl_feas(length(t)) = ctrl_feas(length(t) - 1);
+ stairs(axes_time{5},t,ctrl_feas','DisplayName',sprintf('feas_%d',i));
+ end
+ legend('Location','EastOutside')
+ ylabel('Controller feasible [0/1]')
+
+ % link axes
+ axes_time = [axes_time{:}];
+ linkaxes(axes_time,'x');
+ xlabel('Time [s]')
+
+ % plot without time
+ % YX-plane
+ fig_pos = figure;
+ axes_pos = cell(2,1);
+ axes_pos{1} = subplot(2,1,1);
+ hold on
+ colormap('hsv')
+
+ for i = 1:n_traj
+ plot(axes_pos{1},x(2,:,i),x(1,:,i));
+ end
+
+ axis equal
+ xlabel('Y-Position [m]')
+ ylabel('X-Position [Mm]')
+
+ % YZ plane
+ axes_pos{2} = subplot(2,1,2);
+ hold on
+ for i = 1:n_traj
+ plot(axes_pos{2},x(2,:,i),x(3,:,i))
+ end
+ axis equal
+ xlabel('Y-Position [Mm]')
+ ylabel('Z-Position [Mm]')
+
+ axes_pos = [axes_pos{:}];
+ linkaxes(axes_pos,'x');
+end \ No newline at end of file
diff --git a/templates/plot_trajectory_z.m b/templates/plot_trajectory_z.m
new file mode 100644
index 0000000..1b5a842
--- /dev/null
+++ b/templates/plot_trajectory_z.m
@@ -0,0 +1,78 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% 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.
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+
+function [fig_time,axes_time,fig_pos,axes_pos] = plot_trajectory_z(x,u,ctrl_info,params)
+ fig_time = figure;
+
+ % check if input is 3-dimensional
+ n_traj = size(x,3);
+ assert(size(u,3) == n_traj);
+ assert(size(ctrl_info,3) == n_traj);
+
+ nx = params.model.nx;
+ assert(nx == size(x,1));
+
+ t = 0:params.model.TimeStep:params.model.TimeStep*params.model.HorizonLength;
+
+ % plot
+ axes_time = cell(4,1);
+ axes_time{1} = subplot(4,1,1);
+ hold on;
+
+ for i = 1:n_traj
+ plot(axes_time{1},t,x(1,:,i),'DisplayName',sprintf('z_%d',i));
+ end
+ legend('Location','EastOutside')
+
+ % max position
+ x_max = params.constraints.MaxAbsPositionXZ; %*T(1);
+ plot(axes_time{1}, [t(1); t(end)],[x_max; x_max],'k--','HandleVisibility','off');
+ plot(axes_time{1}, [t(1); t(end)],[-x_max; -x_max],'k--','HandleVisibility','off');
+ ylabel('Position [Mm]')
+
+ axes_time{2} = subplot(4,1,2);
+ hold on;
+ for i = 1:n_traj
+ plot(axes_time{2},t,x(2,:,i),'DisplayName',sprintf('v_{z%d}',i));
+ end
+ legend('Location','EastOutside')
+ ylabel('Velocity [km/s]')
+
+ axes_time{3} = subplot(4,1,3);
+ hold on;
+ % append one input value for plotting
+ u(:,length(t),:) = u(:,length(t)-1,:);
+ for i = 1:n_traj
+ stairs(axes_time{3},t,u(1,:,i)','DisplayName',sprintf('u_{z%d}',i));
+ end
+
+ % max thrust
+ thrust_max = params.constraints.MaxAbsThrust;
+ plot([t(1); t(end)],[thrust_max; thrust_max],'k--','HandleVisibility','off');
+ plot([t(1); t(end)],[-thrust_max; -thrust_max],'k--','HandleVisibility','off');
+ legend('Location','EastOutside')
+ ylabel('Thrust [N]')
+
+ % feasibility
+ axes_time{4} = subplot(4,1,4);
+ hold on;
+ for i = 1:n_traj
+ % append one input value for plotting
+ ctrl_feas = [ctrl_info(:,:,i).ctrl_feas];
+ ctrl_feas(length(t)) = ctrl_feas(length(t) - 1);
+ stairs(axes_time{4},t,ctrl_feas','DisplayName',sprintf('feas_%d',i));
+ end
+ legend('Location','EastOutside')
+ ylabel('Feasible [0/1]')
+
+ % link axes
+ axes_time = [axes_time{:}];
+ linkaxes(axes_time,'x')
+ xlabel('Time [s]')
+end \ No newline at end of file
diff --git a/templates/simulate.m b/templates/simulate.m
new file mode 100644
index 0000000..3138cd1
--- /dev/null
+++ b/templates/simulate.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.
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+function [Xt,Ut,ctrl_info] = simulate(x0, ctrl, params)
+
+% YOUR CODE HERE
+% Hint: you can access the control command with ctrl.eval(x(:,i))
+
+end \ No newline at end of file
diff --git a/templates/simulate_uncertain.m b/templates/simulate_uncertain.m
new file mode 100644
index 0000000..dc9df26
--- /dev/null
+++ b/templates/simulate_uncertain.m
@@ -0,0 +1,11 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% 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.
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+function [Xt,Ut,ctrl_info] = simulate_uncertain(x0, ctrl, Wt, params)
+ % YOUR CODE HERE
+end \ No newline at end of file
diff --git a/templates/traj_constraints.m b/templates/traj_constraints.m
new file mode 100644
index 0000000..4b0a081
--- /dev/null
+++ b/templates/traj_constraints.m
@@ -0,0 +1,12 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% 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.
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+function [s_max, y_max, u_max, J_u, df_max, vf_max, traj_feas] = traj_constraints(x,u,params)
+ % YOUR CODE HERE
+end
+
diff --git a/templates/traj_cost.m b/templates/traj_cost.m
new file mode 100644
index 0000000..7e0b443
--- /dev/null
+++ b/templates/traj_cost.m
@@ -0,0 +1,11 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% 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.
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+function J_Nt = traj_cost(Xt,Ut,Q,R)
+ % YOUR CODE HERE
+end \ No newline at end of file
diff --git a/testing/.MATLABDriveTag b/testing/.MATLABDriveTag
new file mode 100644
index 0000000..91a4f10
--- /dev/null
+++ b/testing/.MATLABDriveTag
@@ -0,0 +1 @@
+da537f63-b054-4058-9b41-6907e21d5531 \ No newline at end of file
diff --git a/testing/TestFunction.p b/testing/TestFunction.p
new file mode 100644
index 0000000..7bb108c
--- /dev/null
+++ b/testing/TestFunction.p
Binary files differ
diff --git a/testing/TestFunction_LQR.p b/testing/TestFunction_LQR.p
new file mode 100644
index 0000000..955ba03
--- /dev/null
+++ b/testing/TestFunction_LQR.p
Binary files differ
diff --git a/testing/TestFunction_MPC.p b/testing/TestFunction_MPC.p
new file mode 100644
index 0000000..b802e23
--- /dev/null
+++ b/testing/TestFunction_MPC.p
Binary files differ
diff --git a/testing/TestFunction_MPC_TE.p b/testing/TestFunction_MPC_TE.p
new file mode 100644
index 0000000..d035c80
--- /dev/null
+++ b/testing/TestFunction_MPC_TE.p
Binary files differ
diff --git a/testing/TestFunction_MPC_TE_forces.p b/testing/TestFunction_MPC_TE_forces.p
new file mode 100644
index 0000000..81ff7ba
--- /dev/null
+++ b/testing/TestFunction_MPC_TE_forces.p
Binary files differ
diff --git a/testing/TestFunction_MPC_TS.p b/testing/TestFunction_MPC_TS.p
new file mode 100644
index 0000000..23c4836
--- /dev/null
+++ b/testing/TestFunction_MPC_TS.p
Binary files differ
diff --git a/testing/TestFunction_MPC_TS_SC.p b/testing/TestFunction_MPC_TS_SC.p
new file mode 100644
index 0000000..faef7e4
--- /dev/null
+++ b/testing/TestFunction_MPC_TS_SC.p
Binary files differ
diff --git a/testing/TestFunction_MPC_TS_SC_script.p b/testing/TestFunction_MPC_TS_SC_script.p
new file mode 100644
index 0000000..e13715e
--- /dev/null
+++ b/testing/TestFunction_MPC_TS_SC_script.p
Binary files differ
diff --git a/testing/TestFunction_MPC_TUBE.p b/testing/TestFunction_MPC_TUBE.p
new file mode 100644
index 0000000..4bac841
--- /dev/null
+++ b/testing/TestFunction_MPC_TUBE.p
Binary files differ
diff --git a/testing/TestFunction_MPC_TUBE_script.p b/testing/TestFunction_MPC_TUBE_script.p
new file mode 100644
index 0000000..9946b16
--- /dev/null
+++ b/testing/TestFunction_MPC_TUBE_script.p
Binary files differ
diff --git a/testing/TestFunction_compute_minRPI.p b/testing/TestFunction_compute_minRPI.p
new file mode 100644
index 0000000..9390f12
--- /dev/null
+++ b/testing/TestFunction_compute_minRPI.p
Binary files differ
diff --git a/testing/TestFunction_compute_tightening.p b/testing/TestFunction_compute_tightening.p
new file mode 100644
index 0000000..d2e60ea
--- /dev/null
+++ b/testing/TestFunction_compute_tightening.p
Binary files differ
diff --git a/testing/TestFunction_compute_tube_controller.p b/testing/TestFunction_compute_tube_controller.p
new file mode 100644
index 0000000..764bd90
--- /dev/null
+++ b/testing/TestFunction_compute_tube_controller.p
Binary files differ
diff --git a/testing/TestFunction_generate_constraints.p b/testing/TestFunction_generate_constraints.p
new file mode 100644
index 0000000..6bff694
--- /dev/null
+++ b/testing/TestFunction_generate_constraints.p
Binary files differ
diff --git a/testing/TestFunction_generate_disturbances.p b/testing/TestFunction_generate_disturbances.p
new file mode 100644
index 0000000..9dc80b6
--- /dev/null
+++ b/testing/TestFunction_generate_disturbances.p
Binary files differ
diff --git a/testing/TestFunction_generate_params.p b/testing/TestFunction_generate_params.p
new file mode 100644
index 0000000..1ab5f90
--- /dev/null
+++ b/testing/TestFunction_generate_params.p
Binary files differ
diff --git a/testing/TestFunction_generate_system.p b/testing/TestFunction_generate_system.p
new file mode 100644
index 0000000..74e80ac
--- /dev/null
+++ b/testing/TestFunction_generate_system.p
Binary files differ
diff --git a/testing/TestFunction_generate_system_cont.p b/testing/TestFunction_generate_system_cont.p
new file mode 100644
index 0000000..8c48d90
--- /dev/null
+++ b/testing/TestFunction_generate_system_cont.p
Binary files differ
diff --git a/testing/TestFunction_generate_system_scaled.p b/testing/TestFunction_generate_system_scaled.p
new file mode 100644
index 0000000..4176219
--- /dev/null
+++ b/testing/TestFunction_generate_system_scaled.p
Binary files differ
diff --git a/testing/TestFunction_lqr_maxPI.p b/testing/TestFunction_lqr_maxPI.p
new file mode 100644
index 0000000..a2fc282
--- /dev/null
+++ b/testing/TestFunction_lqr_maxPI.p
Binary files differ
diff --git a/testing/TestFunction_lqr_tuning.p b/testing/TestFunction_lqr_tuning.p
new file mode 100644
index 0000000..c97d218
--- /dev/null
+++ b/testing/TestFunction_lqr_tuning.p
Binary files differ
diff --git a/testing/TestFunction_lqr_tuning_script.p b/testing/TestFunction_lqr_tuning_script.p
new file mode 100644
index 0000000..09f6896
--- /dev/null
+++ b/testing/TestFunction_lqr_tuning_script.p
Binary files differ
diff --git a/testing/TestFunction_simulate.p b/testing/TestFunction_simulate.p
new file mode 100644
index 0000000..6751e46
--- /dev/null
+++ b/testing/TestFunction_simulate.p
Binary files differ
diff --git a/testing/TestFunction_simulate_uncertain.p b/testing/TestFunction_simulate_uncertain.p
new file mode 100644
index 0000000..9ec7139
--- /dev/null
+++ b/testing/TestFunction_simulate_uncertain.p
Binary files differ
diff --git a/testing/TestFunction_traj_constraints.p b/testing/TestFunction_traj_constraints.p
new file mode 100644
index 0000000..5805983
--- /dev/null
+++ b/testing/TestFunction_traj_constraints.p
Binary files differ
diff --git a/testing/TestFunction_traj_cost.p b/testing/TestFunction_traj_cost.p
new file mode 100644
index 0000000..d05dae0
--- /dev/null
+++ b/testing/TestFunction_traj_cost.p
Binary files differ
diff --git a/testing/linearController.p b/testing/linearController.p
new file mode 100644
index 0000000..ede2acc
--- /dev/null
+++ b/testing/linearController.p
Binary files differ
diff --git a/testing/run_tests.p b/testing/run_tests.p
new file mode 100644
index 0000000..d233f5a
--- /dev/null
+++ b/testing/run_tests.p
Binary files differ