diff options
author | Yuan Xu <yuanxu@student.ethz.ch> | 2023-05-24 17:35:46 +0200 |
---|---|---|
committer | Yuan Xu <yuanxu@student.ethz.ch> | 2023-05-24 17:35:46 +0200 |
commit | a23ba49056a36aff221d0c23e228b981222c3576 (patch) | |
tree | 742c18cbe13c1603909b55ee82188ce6360e8fa0 /Submission files/lqr_maxPI.m | |
parent | ADD: declaration of originality (diff) | |
download | mpc_pe-submission.tar.gz mpc_pe-submission.zip |
add submission zipsubmission
Diffstat (limited to 'Submission files/lqr_maxPI.m')
-rw-r--r-- | Submission files/lqr_maxPI.m | 45 |
1 files changed, 45 insertions, 0 deletions
diff --git a/Submission files/lqr_maxPI.m b/Submission files/lqr_maxPI.m new file mode 100644 index 0000000..99b5b22 --- /dev/null +++ b/Submission files/lqr_maxPI.m @@ -0,0 +1,45 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Copyright (c) 2023, Amon Lahr, Simon Muntwiler, Antoine Leeman & Fabian Flürenbrock Institute for Dynamic Systems and Control, ETH Zurich. +% +% All rights reserved. +% +% 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; + nx=params.model.nx; + nu=params.model.nu; + + s_max= params.constraints.MaxAbsPositionXZ; + y_max= params.constraints.MaxAbsPositionY; + u_max = params.constraints.MaxAbsThrust; + + K = -dlqr(A, B, Q, R); + systemLQR = LTISystem('A', A+B*K); +% absxmax = ones(nx,1)*s_max; +% absumax = ones(nu,1)*u_max; +% Xp = Polyhedron('A',[eye(nx); -eye(nx); K; -K], 'b', [absxmax;absxmax; absumax;absumax]); + Hx=params.constraints.StateMatrix; + hx=params.constraints.StateRHS; + Hu=params.constraints.InputMatrix; + hu=params.constraints.InputRHS; + Xp = Polyhedron('A',[Hx;Hu*K], 'b', [hx;hu]); + + figure(1); + 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; + + +end + + + |