update previous filesyuanxu

9 files changed, 27 insertions, 142 deletions

diff --git a/templates/LQR.m b/templates/LQR.m
index c05237d..d23e64a 100644
--- a/templates/LQR.m
+++ b/templates/LQR.m
@@ -14,20 +14,14 @@ classdef LQR
     methods
         %constructor
         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;
+            [K,~,~] = dlqr(A,B,Q,R);
+            obj.K = K;
         end
         
         function [u, ctrl_info] = eval(obj,x)
-            % YOUR CODE HERE
-            % u = ...
-            u = obj.K*x;
+            u = -obj.K * x;
             ctrl_info = struct('ctrl_feas',true);
         end
     end
diff --git a/templates/MPC.m b/templates/MPC.m
index b2d411d..355eb81 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
@@ -54,7 +16,6 @@ classdef MPC
             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');
@@ -62,7 +23,6 @@ classdef MPC
 
             [K,P,~] = dlqr(A,B,Q,R);
 
-            % define constraints
 %             s_max=params.constraints.MaxAbsPositionXZ;
 %             y_max=params.constraints.MaxAbsPositionY;
 %             u_max = params.constraints.MaxAbsThrust;
@@ -99,17 +59,12 @@ classdef MPC
             [optimizer_out,errorcode,~] = obj.yalmip_optimizer{x};
             solvetime = toc;
 
-            % extract optimal control action and objective function value
-            u = optimizer_out{1};
-            objective = optimizer_out{2};
+            [u, objective] = optimizer_out{:};
 
-            % 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
diff --git a/templates/generate_constraints.m b/templates/generate_constraints.m
index 008f50c..a9f88fe 100644
--- a/templates/generate_constraints.m
+++ b/templates/generate_constraints.m
@@ -15,11 +15,5 @@ function [H_u, h_u, H_x, h_x] = generate_constraints(params)
     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];
+    H_x=[eye(3),zeros(3);-eye(3),zeros(3)];
 end
\ No newline at end of file
diff --git a/templates/generate_params.m b/templates/generate_params.m
index e2e6024..66c9995 100644
--- a/templates/generate_params.m
+++ b/templates/generate_params.m
@@ -54,20 +54,4 @@ 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..01e1b59 100644
--- a/templates/generate_system.m
+++ b/templates/generate_system.m
@@ -8,12 +8,9 @@
 
 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;
+    t=params.model.TimeStep;
+    sys_c=ss(Ac,Bc,[],[]);
+    sys_d = c2d(sys_c, t, 'zoh');
+    A = sys_d.A;
+    B = sys_d.B;
 end
\ No newline at end of file
diff --git a/templates/generate_system_cont.m b/templates/generate_system_cont.m
index 770fd8e..ff8a593 100644
--- a/templates/generate_system_cont.m
+++ b/templates/generate_system_cont.m
@@ -8,15 +8,13 @@
 
 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];
+    wn=sqrt(params.model.GravitationalParameter/(params.model.TargetRadius)^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*wn^2,0,0,0,2*wn,0;
+        0,0,0,-2*wn,0,0;
+        0,0,-wn^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..2d008d2 100644
--- a/templates/generate_system_scaled.m
+++ b/templates/generate_system_scaled.m
@@ -8,8 +8,7 @@
 
 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;
+    V=params.model.ScalingMatrix;
+    A=V*At/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..251d766 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,14 +30,13 @@ 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
diff --git a/templates/simulate.m b/templates/simulate.m
index 6100434..a5525f6 100644
--- a/templates/simulate.m
+++ b/templates/simulate.m
@@ -12,18 +12,15 @@ function [Xt,Ut,ctrl_info] = simulate(x0, ctrl, params)
 % Hint: you can access the control command with ctrl.eval(x(:,i))
     Xt = [x0];
     Ut = [];
-    x = x0
+    x = x0;
     ctrl_info = [];
-    % obj = LQR();
     for i = 1:params.model.HorizonLength
         [u, ctrl_info_] = ctrl.eval(x);
-        Ut = [Ut u];
+        Ut = [Ut,u];
         ctrl_info = [ctrl_info ctrl_info_];
         x = params.model.A*x + params.model.B*u;
-        Xt = [Xt x];
+        Xt = [Xt,x];
     end
     plot_trajectory(Xt, Ut, ctrl_info, params);
-% Ut = u;
-% Xt = x;
 
 end
\ No newline at end of file