diff options
Diffstat (limited to 'templates/lqr_maxPI.m')
-rw-r--r-- | templates/lqr_maxPI.m | 37 |
1 files changed, 35 insertions, 2 deletions
diff --git a/templates/lqr_maxPI.m b/templates/lqr_maxPI.m index efa335a..99b5b22 100644 --- a/templates/lqr_maxPI.m +++ b/templates/lqr_maxPI.m @@ -6,7 +6,40 @@ % Please see the LICENSE file that has been included as part of this package. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -function [H, h] = lqr_maxPI(Q,R,params) - % YOUR CODE HERE + +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 + + |