diff options
Diffstat (limited to '')
| -rw-r--r-- | templates/MPC_TUBE_script.m | 15 | ||||
| -rw-r--r-- | templates/lqr_maxPI.m | 38 |
2 files changed, 17 insertions, 36 deletions
diff --git a/templates/MPC_TUBE_script.m b/templates/MPC_TUBE_script.m new file mode 100644 index 0000000..a021cd0 --- /dev/null +++ b/templates/MPC_TUBE_script.m @@ -0,0 +1,15 @@ +p = [0.05, 0.1]; +params = generate_params(); +params = generate_params_z(params); +K_tube = compute_tube_controller(p,params); +[H_tube, h_tube, n_iter] = compute_minRPI(K_tube, params); +params = compute_tightening(K_tube, H_tube, h_tube, params); + +Q = diag([300,0.1]);R = 1;N = 50; +[H_N, h_N] = lqr_maxPI(Q, R, params); + +% MPC_TUBE_obj = MPC_TUBE(Q,R,N,H_N,h_N,H_tube,h_tube,K_tube,params); +matfile = struct("p", p, "K_tube", K_tube, "H_tube", H_tube, "h_tube", h_tube,... + "H_N", H_N, "h_N", h_N, "params_z_tube", params); +save("MPC_TUBE_params.mat", "-struct", "matfile"); + diff --git a/templates/lqr_maxPI.m b/templates/lqr_maxPI.m index 47ddd37..86d838e 100644 --- a/templates/lqr_maxPI.m +++ b/templates/lqr_maxPI.m @@ -6,36 +6,6 @@ % Please see the LICENSE file that has been included as part of this package. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -% 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 @@ -44,12 +14,10 @@ function [H, h] = lqr_maxPI(Q, R, params) 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; @@ -62,17 +30,15 @@ function [H, h] = lqr_maxPI(Q, R, params) 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 + H=InvSetLQR.A; + h=InvSetLQR.b; end - |