From 0105f2746c6dd8c3617f93fd3281f7e2708cae76 Mon Sep 17 00:00:00 2001 From: Nao Pross Date: Wed, 24 May 2023 16:29:40 +0200 Subject: Take deliverables for from LQR to MPC from yuanxu According to table 7 that is - lqr_maxPI - traj_cost - MPC - MPC/eval (contained in MPC.m) --- templates/lqr_maxPI.m | 37 +++++++++++++++++++++++++++++++++++-- 1 file changed, 35 insertions(+), 2 deletions(-) (limited to 'templates/lqr_maxPI.m') diff --git a/templates/lqr_maxPI.m b/templates/lqr_maxPI.m index efa335a..251d766 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 + + -- cgit v1.2.1 From ace609eb706d907c4997a76849e7a260c3a12b6c Mon Sep 17 00:00:00 2001 From: YanzhenXiangRobotics Date: Wed, 24 May 2023 17:21:48 +0200 Subject: ADD: passed 1-24 --- templates/lqr_maxPI.m | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'templates/lqr_maxPI.m') diff --git a/templates/lqr_maxPI.m b/templates/lqr_maxPI.m index 251d766..99b5b22 100644 --- a/templates/lqr_maxPI.m +++ b/templates/lqr_maxPI.m @@ -33,7 +33,7 @@ function [H, h] = lqr_maxPI(Q, R, params) 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'); +% 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; -- cgit v1.2.1