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
 
+
+