From 4a7a45ea55ecd4d4f1bafec34f55902dbefc363d Mon Sep 17 00:00:00 2001
From: Nao Pross <np@0hm.ch>
Date: Wed, 24 May 2023 16:17:59 +0200
Subject: Take deliverables for system modelling from npross

According to table 3 that is

  - generate_system_cont
  - generate_system
  - generate_system_scaled
  - generate_contraints
  - generate_params
---
 templates/generate_constraints.m   |  7 ++++++-
 templates/generate_params.m        | 15 ++++++++++++++-
 templates/generate_system.m        |  8 ++++++--
 templates/generate_system_cont.m   | 15 +++++++++++++--
 templates/generate_system_scaled.m |  9 +++++++--
 5 files changed, 46 insertions(+), 8 deletions(-)

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_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
-- 
cgit v1.2.1