diff options
| -rw-r--r-- | templates/lqr_maxPI.m | 70 |
1 files changed, 68 insertions, 2 deletions
diff --git a/templates/lqr_maxPI.m b/templates/lqr_maxPI.m index efa335a..47ddd37 100644 --- a/templates/lqr_maxPI.m +++ b/templates/lqr_maxPI.m @@ -6,7 +6,73 @@ % 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; +% +% % 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 + A = params.model.A; + B = params.model.B; + 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; +% 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); +% 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 + + end + + |