summaryrefslogtreecommitdiffstats
path: root/templates
diff options
context:
space:
mode:
Diffstat (limited to '')
-rw-r--r--templates/LQR.m7
-rw-r--r--templates/MPC.m89
-rw-r--r--templates/dummy_test.m0
-rw-r--r--templates/generate_constraints.m14
-rw-r--r--templates/generate_params.m28
-rw-r--r--templates/generate_system.m8
-rw-r--r--templates/generate_system_cont.m11
-rw-r--r--templates/generate_system_scaled.m4
-rw-r--r--templates/lqr_maxPI.m70
-rw-r--r--templates/lqr_tuning.m50
-rw-r--r--templates/simulate.m15
-rw-r--r--templates/traj_constraints.m31
-rw-r--r--templates/traj_cost.m6
13 files changed, 11 insertions, 322 deletions
diff --git a/templates/LQR.m b/templates/LQR.m
index c05237d..2a79da9 100644
--- a/templates/LQR.m
+++ b/templates/LQR.m
@@ -16,18 +16,11 @@ classdef LQR
function obj = LQR(Q,R,params)
% YOUR CODE HERE
% obj.K = ... (save feedback matrix for use in eval function)
- A = params.model.A;
- B = params.model.B;
- % [X, L, G] = dare(A,B,Q,R);
- [K,S,e] = dlqr(A,B,Q,R)
- % obj.K = -inv(B'*P_inf*B+R)*B'*P_inf*A;
- obj.K = -K;
end
function [u, ctrl_info] = eval(obj,x)
% YOUR CODE HERE
% u = ...
- u = obj.K*x;
ctrl_info = struct('ctrl_feas',true);
end
end
diff --git a/templates/MPC.m b/templates/MPC.m
index b2d411d..3e9d2f1 100644
--- a/templates/MPC.m
+++ b/templates/MPC.m
@@ -6,44 +6,6 @@
% Please see the LICENSE file that has been included as part of this package.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-% classdef MPC
-% properties
-% yalmip_optimizer
-% end
-%
-% methods
-% function obj = MPC(Q,R,N,params)
-% nu = params.model.nu;
-% nx = params.model.nx;
-%
-% % define optimization variables
-% U = sdpvar(repmat(nu,1,N),ones(1,N),'full');
-% X0 = sdpvar(nx,1,'full');
-%
-% % YOUR CODE HERE
-%
-% opts = sdpsettings('verbose',1,'solver','quadprog');
-% obj.yalmip_optimizer = optimizer(constraints,objective,opts,X0,{U{1} objective});
-% end
-%
-% function [u, ctrl_info] = eval(obj,x)
-% %% evaluate control action by solving MPC problem, e.g.
-% tic;
-% [optimizer_out,errorcode] = obj.yalmip_optimizer(x);
-% solvetime = toc;
-%
-% [u, objective] = optimizer_out{:};
-%
-% feasible = true;
-% if (errorcode ~= 0)
-% feasible = false;
-% end
-%
-% ctrl_info = struct('ctrl_feas',feasible,'objective',objective,'solvetime',solvetime);
-% end
-% end
-% end
-
classdef MPC
properties
yalmip_optimizer
@@ -53,64 +15,31 @@ classdef MPC
function obj = MPC(Q,R,N,params)
nu = params.model.nu;
nx = params.model.nx;
- % YOUR CODE HERE
+
% define optimization variables
- A=params.model.A;
- B=params.model.B;
U = sdpvar(repmat(nu,1,N),ones(1,N),'full');
- X = sdpvar(repmat(nx,1,N+1),ones(1,N+1),'full');
-
- [K,P,~] = dlqr(A,B,Q,R);
-
- % define constraints
-% s_max=params.constraints.MaxAbsPositionXZ;
-% y_max=params.constraints.MaxAbsPositionY;
-% u_max = params.constraints.MaxAbsThrust;
- H_x = params.constraints.StateMatrix;
- h_x = params.constraints.StateRHS;
- H_u = params.constraints.InputMatrix;
- h_u = params.constraints.InputRHS;
X0 = sdpvar(nx,1,'full');
- objective = 0;
- constraints = X{1} == X0;
- for k = 1:N
- constraints = [ ...
- constraints, ...
- X{k+1} == A*X{k} + B*U{k} , ...
- H_x * X{k} <= h_x, ...
- H_u * U{k} <= h_u ...
- ];
- objective = objective + X{k}'*Q*X{k} + U{k}'*R*U{k};
- end
- objective=objective+X{N+1}'*P*X{N+1};
- % terminal constraint
-% constraints = [ ...
-% constraints, ...
-% X{N+1} == zeros(nx,1)
-% ];
+ % YOUR CODE HERE
+
opts = sdpsettings('verbose',1,'solver','quadprog');
obj.yalmip_optimizer = optimizer(constraints,objective,opts,X0,{U{1} objective});
end
function [u, ctrl_info] = eval(obj,x)
- % evaluate control action by solving MPC problem
+ %% evaluate control action by solving MPC problem, e.g.
tic;
- [optimizer_out,errorcode,~] = obj.yalmip_optimizer{x};
+ [optimizer_out,errorcode] = obj.yalmip_optimizer(x);
solvetime = toc;
+
+ [u, objective] = optimizer_out{:};
- % extract optimal control action and objective function value
- u = optimizer_out{1};
- objective = optimizer_out{2};
-
- % check feasibility of optimization problem
- feasible = ~isnan(objective) && ~isinf(objective);
+ feasible = true;
if (errorcode ~= 0)
feasible = false;
end
- % create control info struct
ctrl_info = struct('ctrl_feas',feasible,'objective',objective,'solvetime',solvetime);
end
end
-end
+end \ No newline at end of file
diff --git a/templates/dummy_test.m b/templates/dummy_test.m
deleted file mode 100644
index e69de29..0000000
--- a/templates/dummy_test.m
+++ /dev/null
diff --git a/templates/generate_constraints.m b/templates/generate_constraints.m
index 008f50c..892b706 100644
--- a/templates/generate_constraints.m
+++ b/templates/generate_constraints.m
@@ -8,18 +8,4 @@
function [H_u, h_u, H_x, h_x] = generate_constraints(params)
% YOUR CODE HERE
- u_max = params.constraints.MaxAbsThrust;
- h_u = u_max * ones(6,1);
- H_u = [eye(3);-eye(3)];
-
- s_max = params.constraints.MaxAbsPositionXZ;
- y_max = params.constraints.MaxAbsPositionY;
- h_x = [s_max;y_max;s_max;s_max;y_max;s_max];
-% H_x = [eye(3);-eye(3)];
- H_x=[1,0,0,0,0,0;
- 0,1,0,0,0,0;
- 0,0,1,0,0,0;
- -1,0,0,0,0,0;
- 0,-1,0,0,0,0;
- 0,0,-1,0,0,0];
end \ No newline at end of file
diff --git a/templates/generate_params.m b/templates/generate_params.m
index e2e6024..ef51366 100644
--- a/templates/generate_params.m
+++ b/templates/generate_params.m
@@ -41,33 +41,5 @@ params.exercise = struct( ...
);
% YOUR CODE HERE
-[Ac, Bc] = generate_system_cont(params);
-[A_tilta, B_tilta] = generate_system(Ac, Bc, params);
-[A, B] = generate_system_scaled(A_tilta, B_tilta, params);
-[Hu, hu, Hx, hx] = generate_constraints(params);
-
-params.model.A = A;
-params.model.B = B;
-
-params.constraints.InputMatrix = Hu;
-params.constraints.InputRHS = hu;
-params.constraints.StateMatrix = Hx;
-params.constraints.StateRHS = hx;
-
-% new_model_struct = struct(...
-% 'A', A, ...
-% 'B', B ...
-% );
-
-% new_constr_struct = struct(...
-% 'InputMatrix', Hu,...
-% 'InputRHS', hu,...
-% 'StateMatrix', Hx,...
-% 'StateRHS', hx ...
-% )
-
-
-% params.model = [params.model new_model_struct];
-% params.constraints = [params.constraints, new_constr_struct];
end
diff --git a/templates/generate_system.m b/templates/generate_system.m
index c4dadc2..9deb347 100644
--- a/templates/generate_system.m
+++ b/templates/generate_system.m
@@ -8,12 +8,4 @@
function [A, B] = generate_system(Ac, Bc, params)
% YOUR CODE HERE
- % Ts = 600;
- sysc = ss(Ac, Bc, [], []);
- Ts = params.model.TimeStep;
- sysd = c2d(sysc,Ts);
- % A = eye(params.model.nx) + Ac * Ts;
- % B = Bc * Ts;
- A = sysd.A;
- B = sysd.B;
end \ No newline at end of file
diff --git a/templates/generate_system_cont.m b/templates/generate_system_cont.m
index 770fd8e..2d3ee79 100644
--- a/templates/generate_system_cont.m
+++ b/templates/generate_system_cont.m
@@ -8,15 +8,4 @@
function [Ac, Bc] = generate_system_cont(params)
% YOUR CODE HERE
- miu = params.model.GravitationalParameter;
- r = params.model.TargetRadius;
- omega_n = sqrt(miu/(r^3));
- m = params.model.Mass;
- Ac = [0,0,0,1,0,0;
- 0,0,0,0,1,0;
- 0,0,0,0,0,1;
- 3*omega_n^2,0,0,0,2*omega_n,0;
- 0,0,0,-2*omega_n,0,0;
- 0,0,-omega_n^2,0,0,0];
- Bc = [zeros(3,3);1/m*eye(3)];
end \ No newline at end of file
diff --git a/templates/generate_system_scaled.m b/templates/generate_system_scaled.m
index 38714d2..eac8db8 100644
--- a/templates/generate_system_scaled.m
+++ b/templates/generate_system_scaled.m
@@ -8,8 +8,4 @@
function [A,B] = generate_system_scaled(At,Bt,params)
% YOUR CODE HERE
- % V = diag([1e-6, 1e-6, 1e-6, 1e-3, 1e-3, 1e-3]);
- V = params.model.ScalingMatrix;
- A = V*At*inv(V);
- B = V*Bt;
end \ No newline at end of file
diff --git a/templates/lqr_maxPI.m b/templates/lqr_maxPI.m
index 47ddd37..efa335a 100644
--- a/templates/lqr_maxPI.m
+++ b/templates/lqr_maxPI.m
@@ -6,73 +6,7 @@
% 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
- 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
-
-
+function [H, h] = lqr_maxPI(Q,R,params)
+ % YOUR CODE HERE
end
-
-
diff --git a/templates/lqr_tuning.m b/templates/lqr_tuning.m
index dc7161b..4a26158 100644
--- a/templates/lqr_tuning.m
+++ b/templates/lqr_tuning.m
@@ -8,54 +8,4 @@
function [tuning_struct, i_opt] = lqr_tuning(x0,Q,params)
% YOUR CODE HERE
- R = eye(params.model.nu);
- size_Q = size(Q);
- param_choices = size_Q(2);
- J_array = [];
- tuning_struct = [];
- for i = 1:param_choices
- Q_mat = diag(Q(:,i));
- ctrl = LQR(Q_mat,R,params);
- [Xt, Ut, ~] = simulate(x0, ctrl, params);
- [s_max, y_max, u_max, J_u, df_max, vf_max, traj_feas] = traj_constraints(Xt,Ut,params);
- % YOUR CODE HERE
- % ctrl_feas_arr = [];
- % for k = 1:params.model.HorizonLength
- % ctrl_feas_arr = [ctrl_feas_arr,ctrl_info(k).ctrl_feas];
- % end
- % if all(ctrl_feas_arr)
- % J = 0;
- % for j = 1:params.model.HorizonLength
- % J = J + Xt(:,j)'*Q_mat*Xt(:,j);
- % J = J + Ut(:,j)'*R*Ut(:,j);
- % end
- % J = J + Xt(:,end)'*Q_mat*Xt(:,end);
- % J_array = [J_array, J];
- % tuning_struct_item.TrajFeasible = true;
- % else
- % tuning_struct_item.TrajFeasible = false;
- % end
- tuning_struct_item.InitialCondition = x0;
- tuning_struct_item.Qdiag = Q(:,i);
- tuning_struct_item.MaxAbsPositionXZ = s_max;
- tuning_struct_item.MaxAbsPositionY = y_max;
- tuning_struct_item.MaxAbsThrust = u_max;
- tuning_struct_item.InputCost = J_u;
- tuning_struct_item.MaxFinalPosDiff = df_max;
- tuning_struct_item.MaxFinalVelDiff = vf_max;
- tuning_struct_item.TrajFeasible = traj_feas;
-
- if traj_feas
- J_array = [J_array, J_u];
- end
-
- tuning_struct = [tuning_struct,tuning_struct_item];
- end
-% display(J_array)
- tuning_struct = tuning_struct';
- if isempty(J_array)
- i_opt = nan;
- else
- [~, i_opt] = min(J_array);
- end
end \ No newline at end of file
diff --git a/templates/simulate.m b/templates/simulate.m
index 6100434..3138cd1 100644
--- a/templates/simulate.m
+++ b/templates/simulate.m
@@ -10,20 +10,5 @@ function [Xt,Ut,ctrl_info] = simulate(x0, ctrl, params)
% YOUR CODE HERE
% Hint: you can access the control command with ctrl.eval(x(:,i))
- Xt = [x0];
- Ut = [];
- x = x0
- ctrl_info = [];
- % obj = LQR();
- for i = 1:params.model.HorizonLength
- [u, ctrl_info_] = ctrl.eval(x);
- Ut = [Ut u];
- ctrl_info = [ctrl_info ctrl_info_];
- x = params.model.A*x + params.model.B*u;
- Xt = [Xt x];
- end
- plot_trajectory(Xt, Ut, ctrl_info, params);
-% Ut = u;
-% Xt = x;
end \ No newline at end of file
diff --git a/templates/traj_constraints.m b/templates/traj_constraints.m
index d5e0c65..4b0a081 100644
--- a/templates/traj_constraints.m
+++ b/templates/traj_constraints.m
@@ -8,36 +8,5 @@
function [s_max, y_max, u_max, J_u, df_max, vf_max, traj_feas] = traj_constraints(x,u,params)
% YOUR CODE HERE
- traj_feas = true;
- % timesteps = params.model.TimeStep;
- size_u = size(u);
- timesteps = size_u(end);
- s_max = max(max(abs(x(1,:))),max(abs(x(3,:))));
- y_max = max(abs(x(2,:)));
- u_max = max(abs(u));
- J_u = 0;
- % fprintf("timestep: %s\n\n\n",timesteps);
-% fprintf("u shape: %s\n\n\n",size(u));
- % display(size(x))
- for k = 1:timesteps
- J_u = J_u + u(:,k)'*u(:,k);
- end
- df_max = sqrt(sum(x(1:3,end).^2));
- vf_max = sqrt(sum(x(4:6,end).^2));
- if s_max > params.constraints.MaxAbsPositionXZ
- traj_feas = false;
- end
- if y_max > params.constraints.MaxAbsPositionY
- traj_feas = false;
- end
- if u_max > params.constraints.MaxAbsThrust
- traj_feas = false;
- end
- if df_max > params.constraints.MaxFinalPosDiff
- traj_feas = false;
- end
- if vf_max > params.constraints.MaxFinalVelDiff
- traj_feas = false;
- end
end
diff --git a/templates/traj_cost.m b/templates/traj_cost.m
index 57566c9..7e0b443 100644
--- a/templates/traj_cost.m
+++ b/templates/traj_cost.m
@@ -8,10 +8,4 @@
function J_Nt = traj_cost(Xt,Ut,Q,R)
% YOUR CODE HERE
- J_Nt=0;
- for i=1:length(Xt(1,:))-1
- a= Xt(:,i)'*Q*Xt(:,i);
- b= Ut(:,i)'*R*Ut(:,i);
- J_Nt=J_Nt+a+b;
- end
end \ No newline at end of file
generated by cgit v1.2.1 (git 2.18.0) at 2024-11-26 00:54:23 +0100