From feb6ec844cf2d0cdb07acd609615675141e2706a Mon Sep 17 00:00:00 2001 From: Yuan Xu Date: Thu, 11 May 2023 12:00:59 +0200 Subject: update previous files --- templates/LQR.m | 12 +++------- templates/MPC.m | 49 ++------------------------------------ templates/generate_constraints.m | 8 +------ templates/generate_params.m | 16 ------------- templates/generate_system.m | 13 ++++------ templates/generate_system_cont.m | 18 +++++++------- templates/generate_system_scaled.m | 7 +++--- templates/lqr_maxPI.m | 37 ++-------------------------- templates/simulate.m | 9 +++---- 9 files changed, 27 insertions(+), 142 deletions(-) diff --git a/templates/LQR.m b/templates/LQR.m index c05237d..d23e64a 100644 --- a/templates/LQR.m +++ b/templates/LQR.m @@ -14,20 +14,14 @@ classdef LQR methods %constructor function obj = LQR(Q,R,params) - % YOUR CODE HERE - % obj.K = ... (save feedback matrix for use in eval function) A = params.model.A; B = params.model.B; - % [X, L, G] = dare(A,B,Q,R); - [K,S,e] = dlqr(A,B,Q,R) - % obj.K = -inv(B'*P_inf*B+R)*B'*P_inf*A; - obj.K = -K; + [K,~,~] = dlqr(A,B,Q,R); + obj.K = K; end function [u, ctrl_info] = eval(obj,x) - % YOUR CODE HERE - % u = ... - u = obj.K*x; + u = -obj.K * x; ctrl_info = struct('ctrl_feas',true); end end diff --git a/templates/MPC.m b/templates/MPC.m index b2d411d..355eb81 100644 --- a/templates/MPC.m +++ b/templates/MPC.m @@ -6,44 +6,6 @@ % Please see the LICENSE file that has been included as part of this package. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -% classdef MPC -% properties -% yalmip_optimizer -% end -% -% methods -% function obj = MPC(Q,R,N,params) -% nu = params.model.nu; -% nx = params.model.nx; -% -% % define optimization variables -% U = sdpvar(repmat(nu,1,N),ones(1,N),'full'); -% X0 = sdpvar(nx,1,'full'); -% -% % YOUR CODE HERE -% -% opts = sdpsettings('verbose',1,'solver','quadprog'); -% obj.yalmip_optimizer = optimizer(constraints,objective,opts,X0,{U{1} objective}); -% end -% -% function [u, ctrl_info] = eval(obj,x) -% %% evaluate control action by solving MPC problem, e.g. -% tic; -% [optimizer_out,errorcode] = obj.yalmip_optimizer(x); -% solvetime = toc; -% -% [u, objective] = optimizer_out{:}; -% -% feasible = true; -% if (errorcode ~= 0) -% feasible = false; -% end -% -% ctrl_info = struct('ctrl_feas',feasible,'objective',objective,'solvetime',solvetime); -% end -% end -% end - classdef MPC properties yalmip_optimizer @@ -54,7 +16,6 @@ classdef MPC nu = params.model.nu; nx = params.model.nx; % YOUR CODE HERE - % define optimization variables A=params.model.A; B=params.model.B; U = sdpvar(repmat(nu,1,N),ones(1,N),'full'); @@ -62,7 +23,6 @@ classdef MPC [K,P,~] = dlqr(A,B,Q,R); - % define constraints % s_max=params.constraints.MaxAbsPositionXZ; % y_max=params.constraints.MaxAbsPositionY; % u_max = params.constraints.MaxAbsThrust; @@ -99,17 +59,12 @@ classdef MPC [optimizer_out,errorcode,~] = obj.yalmip_optimizer{x}; solvetime = toc; - % extract optimal control action and objective function value - u = optimizer_out{1}; - objective = optimizer_out{2}; + [u, objective] = optimizer_out{:}; - % check feasibility of optimization problem - feasible = ~isnan(objective) && ~isinf(objective); + feasible = true; if (errorcode ~= 0) feasible = false; end - - % create control info struct ctrl_info = struct('ctrl_feas',feasible,'objective',objective,'solvetime',solvetime); end end diff --git a/templates/generate_constraints.m b/templates/generate_constraints.m index 008f50c..a9f88fe 100644 --- a/templates/generate_constraints.m +++ b/templates/generate_constraints.m @@ -15,11 +15,5 @@ function [H_u, h_u, H_x, h_x] = generate_constraints(params) 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)]; - H_x=[1,0,0,0,0,0; - 0,1,0,0,0,0; - 0,0,1,0,0,0; - -1,0,0,0,0,0; - 0,-1,0,0,0,0; - 0,0,-1,0,0,0]; + H_x=[eye(3),zeros(3);-eye(3),zeros(3)]; end \ No newline at end of file diff --git a/templates/generate_params.m b/templates/generate_params.m index e2e6024..66c9995 100644 --- a/templates/generate_params.m +++ b/templates/generate_params.m @@ -54,20 +54,4 @@ 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 c4dadc2..01e1b59 100644 --- a/templates/generate_system.m +++ b/templates/generate_system.m @@ -8,12 +8,9 @@ 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; + t=params.model.TimeStep; + sys_c=ss(Ac,Bc,[],[]); + sys_d = c2d(sys_c, t, 'zoh'); + A = sys_d.A; + B = sys_d.B; end \ No newline at end of file diff --git a/templates/generate_system_cont.m b/templates/generate_system_cont.m index 770fd8e..ff8a593 100644 --- a/templates/generate_system_cont.m +++ b/templates/generate_system_cont.m @@ -8,15 +8,13 @@ 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]; + wn=sqrt(params.model.GravitationalParameter/(params.model.TargetRadius)^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*wn^2,0,0,0,2*wn,0; + 0,0,0,-2*wn,0,0; + 0,0,-wn^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 38714d2..2d008d2 100644 --- a/templates/generate_system_scaled.m +++ b/templates/generate_system_scaled.m @@ -8,8 +8,7 @@ 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; + V=params.model.ScalingMatrix; + A=V*At/V; + B=V*Bt; end \ No newline at end of file diff --git a/templates/lqr_maxPI.m b/templates/lqr_maxPI.m index 47ddd37..251d766 100644 --- a/templates/lqr_maxPI.m +++ b/templates/lqr_maxPI.m @@ -6,36 +6,6 @@ % Please see the LICENSE file that has been included as part of this package. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -% function [H, h] = lqr_maxPI(Q, R, params) -% % Define the linear system -% A = params.model.A; -% B = params.model.B; -% -% % Define the state and input constraints -% s_max=params.constraints.MaxAbsPositionXZ; -% y_max=params.constraints.MaxAbsPositionY; -% u_max = params.constraints.MaxAbsThrust; -% -% % Define the cost function -% [K, ~, ~] = dlqr(A, B, Q, R); -% P = dare(A, B, Q, R) -% F = -inv((B'*P*B+R))*B'*P*A -% A -% B -% Ax=[1,0; -% 0,1; -% -1,0; -% 0,-1]; -% Ax=[1,0; -% -1,0; -% 0,1; -% 0,-1]; -% Au=Ax; -% bx=[s_max;s_max;s_max;s_max]; -% bu=[u_max;u_max;u_max;u_max]; -% H=[Ax;Au*F]/2 -% h=[bx;bu] -% end function [H, h] = lqr_maxPI(Q, R, params) % Define the linear system @@ -44,12 +14,10 @@ function [H, h] = lqr_maxPI(Q, R, params) nx=params.model.nx; nu=params.model.nu; - % Define the state and input constraints s_max= params.constraints.MaxAbsPositionXZ; y_max= params.constraints.MaxAbsPositionY; u_max = params.constraints.MaxAbsThrust; - % Define the cost function K = -dlqr(A, B, Q, R); systemLQR = LTISystem('A', A+B*K); % absxmax = ones(nx,1)*s_max; @@ -62,14 +30,13 @@ function [H, h] = lqr_maxPI(Q, R, params) Xp = Polyhedron('A',[Hx;Hu*K], 'b', [hx;hu]); figure(1); -% Xp.plot(), alpha(0.25), title('Resulting State Constraints under LQR Control'), xlabel('x_1'), ylabel('x_2'), zlabel('x_3'); systemLQR.x.with('setConstraint'); systemLQR.x.setConstraint = Xp; InvSetLQR = systemLQR.invariantSet(); InvSetLQR.plot(), alpha(0.25), title('Invariant Set for Triple Integrator under LQR Control'), xlabel('x_1'), ylabel('x_2'), zlabel('x_3'); - H=InvSetLQR.A - h=InvSetLQR.b + H=InvSetLQR.A; + h=InvSetLQR.b; end diff --git a/templates/simulate.m b/templates/simulate.m index 6100434..a5525f6 100644 --- a/templates/simulate.m +++ b/templates/simulate.m @@ -12,18 +12,15 @@ function [Xt,Ut,ctrl_info] = simulate(x0, ctrl, params) % Hint: you can access the control command with ctrl.eval(x(:,i)) Xt = [x0]; Ut = []; - x = x0 + x = x0; ctrl_info = []; - % obj = LQR(); for i = 1:params.model.HorizonLength [u, ctrl_info_] = ctrl.eval(x); - Ut = [Ut u]; + Ut = [Ut,u]; ctrl_info = [ctrl_info ctrl_info_]; x = params.model.A*x + params.model.B*u; - Xt = [Xt x]; + Xt = [Xt,x]; end plot_trajectory(Xt, Ut, ctrl_info, params); -% Ut = u; -% Xt = x; end \ No newline at end of file -- cgit v1.2.1