summaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
Diffstat (limited to '')
-rw-r--r--templates/generate_constraints.m7
-rw-r--r--templates/generate_params.m15
-rw-r--r--templates/generate_system.m8
-rw-r--r--templates/generate_system_cont.m15
-rw-r--r--templates/generate_system_scaled.m9
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