summaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
-rw-r--r--templates/lqr_maxPI.m70
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
+
+