Revert to handout

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