summaryrefslogtreecommitdiffstats
path: root/templates
diff options
context:
space:
mode:
authorYanzhenXiangRobotics <xyz000327@gmail.com>2023-04-25 20:28:44 +0200
committerYanzhenXiangRobotics <xyz000327@gmail.com>2023-04-25 20:28:44 +0200
commit55a42b2be769bbfbd5b697338c6729f06a3d17cc (patch)
treefa380bdc758ac73fc63d4e938dd97fff0ce314fb /templates
parentADD: init proj (diff)
downloadmpc_pe-55a42b2be769bbfbd5b697338c6729f06a3d17cc.tar.gz
mpc_pe-55a42b2be769bbfbd5b697338c6729f06a3d17cc.zip
ADD: pass system modeling test
Diffstat (limited to 'templates')
-rw-r--r--templates/generate_constraints.m8
-rw-r--r--templates/generate_params.m28
-rw-r--r--templates/generate_system.m8
-rw-r--r--templates/generate_system_cont.m11
-rw-r--r--templates/generate_system_scaled.m4
5 files changed, 59 insertions, 0 deletions
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