diff options
Diffstat (limited to '')
| -rw-r--r-- | templates/LQR.m | 9 | ||||
| -rw-r--r-- | templates/generate_constraints.m | 7 | ||||
| -rw-r--r-- | templates/generate_params.m | 15 | ||||
| -rw-r--r-- | templates/generate_system.m | 8 | ||||
| -rw-r--r-- | templates/generate_system_cont.m | 15 | ||||
| -rw-r--r-- | templates/generate_system_scaled.m | 9 | ||||
| -rw-r--r-- | templates/simulate.m | 15 | ||||
| -rw-r--r-- | templates/traj_constraints.m | 27 |
8 files changed, 91 insertions, 14 deletions
diff --git a/templates/LQR.m b/templates/LQR.m index 2a79da9..76d95c6 100644 --- a/templates/LQR.m +++ b/templates/LQR.m @@ -15,13 +15,16 @@ classdef LQR %constructor function obj = LQR(Q,R,params) % YOUR CODE HERE - % obj.K = ... (save feedback matrix for use in eval function) + E = np.zeros(params.model.nx); + S = np.zeros(params.model.nx); + Pinf = idare(params.model.A, params.model.B, Q, R, S, E, 'noscaling') + obj.K = -inv(R + B' * Pinf * B) * B' * Pinf * A; end function [u, ctrl_info] = eval(obj,x) % YOUR CODE HERE - % u = ... + u = obj.K * x; ctrl_info = struct('ctrl_feas',true); end end -end
\ No newline at end of file +end 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 diff --git a/templates/simulate.m b/templates/simulate.m index 3138cd1..6be40c3 100644 --- a/templates/simulate.m +++ b/templates/simulate.m @@ -7,8 +7,17 @@ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function [Xt,Ut,ctrl_info] = simulate(x0, ctrl, params) + % YOUR CODE HERE + % Hint: you can access the control command with ctrl.eval(x(:,i)) -% YOUR CODE HERE -% Hint: you can access the control command with ctrl.eval(x(:,i)) + Xt = zeros(params.model.nx, params.model.HorizonLength+1); + Ut = zeros(params.model.nu, params.model.HorizonLength); + ctrl_info = repmat(struct('ctrl_feas',true), 1, params.model.HorizonLength); -end
\ No newline at end of file + Xt(:,1) = x0; + for k=1:params.model.HorizonLength + [u, ~] = ctrl.eval(Xt(:,k)); + Ut(:,k) = u; + Xt(:,k+1) = params.model.A * Xt(:,k) + params.model.B * Ut(:,k); + end +end diff --git a/templates/traj_constraints.m b/templates/traj_constraints.m index 4b0a081..c87bed6 100644 --- a/templates/traj_constraints.m +++ b/templates/traj_constraints.m @@ -8,5 +8,32 @@ function [s_max, y_max, u_max, J_u, df_max, vf_max, traj_feas] = traj_constraints(x,u,params) % YOUR CODE HERE + + s_max = max(max(abs(x([1,3],:)))); + y_max = max(abs(x(2,:))); + u_max = max(abs(u)); + + J_u = 0; + for k=1:size(u,2) + J_u = J_u + u(:,k)' * u(:,k); + end + + df_max = sqrt(sum(x(1:3,end).^2)); + vf_max = sqrt(sum(x(4:6,end).^2)); + + constr = [ + s_max <= params.constraints.MaxAbsPositionXZ, ... + y_max <= params.constraints.MaxAbsPositionY, ... + df_max <= params.constraints.MaxFinalPosDiff, ... + vf_max <= params.constraints.MaxFinalVelDiff + ]; + + % disp([s_max, y_max, df_max, vf_max]) + % disp([params.constraints.MaxAbsPositionXZ, ... + % params.constraints.MaxAbsPositionY, ... + % params.constraints.MaxFinalPosDiff, ... + % params.constraints.MaxFinalVelDiff]) + % disp(constr') + traj_feas = all(constr); end |