From 55a42b2be769bbfbd5b697338c6729f06a3d17cc Mon Sep 17 00:00:00 2001
From: YanzhenXiangRobotics <xyz000327@gmail.com>
Date: Tue, 25 Apr 2023 20:28:44 +0200
Subject: ADD: pass system modeling test

---
 templates/generate_constraints.m   |  8 ++++++++
 templates/generate_params.m        | 28 ++++++++++++++++++++++++++++
 templates/generate_system.m        |  8 ++++++++
 templates/generate_system_cont.m   | 11 +++++++++++
 templates/generate_system_scaled.m |  4 ++++
 5 files changed, 59 insertions(+)

(limited to 'templates')

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