From 4a7a45ea55ecd4d4f1bafec34f55902dbefc363d Mon Sep 17 00:00:00 2001 From: Nao Pross 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