summaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
authorYanzhenXiangRobotics <xyz000327@gmail.com>2023-05-15 17:02:21 +0200
committerYanzhenXiangRobotics <xyz000327@gmail.com>2023-05-15 17:02:21 +0200
commit1afec60c8acd4f53ef73b3bf0c76bf729a7416a2 (patch)
tree0aca3df1819b3af47ffafd4e6a905090c3ae1377
parentADD: pass 30 (diff)
downloadmpc_pe-yanzhen.tar.gz
mpc_pe-yanzhen.zip
ADD: pass 31yanzhen
Diffstat (limited to '')
-rw-r--r--templates/MPC_TUBE_script.m15
-rw-r--r--templates/lqr_maxPI.m38
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
-