Update ttr_robustMPC.m

Verified_Examples/TTR/ttr_robustMPC.m

@@ -6,12 +6,10 @@
 
 %%  Load TTR Parameters
 Ts = 0.25;
+
 run example_ttr_model
 
 %%  Define Additive Uncertain Model
-E_x = 0.1*B_x;
-E_y = 0.1*B_y;
-E_z = 0.1*B_z;
 E_x = [0 1 0]'; E_y = [0 1 0]';E_z = [0 .03 0]';
 
 add_usys_x_d = add_uss(A_x,B_x,C_x,D_x,E_x,Ts)
@@ -66,57 +64,43 @@
 W_x_bounds = [-0.25 0.25];
 N = 2; 
 
-%Y_x_Limit_orig = [Inf 1 pi/4 pi];
-Y_x_Limit_orig = [Inf 1 deg2rad(15)]; % x 
 Y_x_Limit_orig = [Inf 1 deg2rad(45)]; % y 
-% Y_x_Limit_orig = [Inf 2 5]; % z
-% Y_x_Limit_orig = [Inf 1 1];
 %Y_x_Limit = expand_Yconstraints(Y_x_Limit_orig,N)
 
 U_x_bounds = [-deg2rad(10) deg2rad(10)]; % x_controller
 U_x_bounds = [-deg2rad(15) deg2rad(15)]; % y_controller
-% U_x_bounds = [-5 5]; % z_controller
 
 %   norm_type = 1;
-Q = diag([25 .5 .1]); % x_controller
-Q = .5*diag([25 .5 .1]); % x_controller
-% Q = diag([20 .2 .05]); % y_controller
-Q = diag([20 1.2 .05]); % y_controller
 Q = .8*diag([16 1.4 .005]); % y_controller
-%Q = diag([11.5 .34 .1]); % y_controller
-% Q = diag([88 11.8 .55]); % z_controller
-%Q = diag([10000 1.8 .1]); % z_controller
 
 % %   norm_type = inf
 % Q = diag([22 1.85 .1]); % x_controller
 % % Q = diag([21 2.5 .1]); % y_controller
-% % Q = diag([40 8.8 1.05]); % z_controller
 % % Q = diag([38 9.8 1.05]); % z_controller
 
 
-R = .15; % x_controller
-R = 1.25*.15; % x_controller
-R = 20*.15; % y_controller
-%R = 3.5*.15; % y_controller
-%R = .125; % z_controller
-% R = .125; % z_controller
+R = 20*.15; 
 
 Y_ref = [0 0 0]';
 
 [sol_x_mp,ValueFunction_x,MP_SolutionOut] = MP_CL_MinMax_SDPrelax(add_usys_d, x_state, Y_ref, Y_x_Limit_orig,U_x_bounds,W_x_bounds,Q,R,N,norm_type)
 
 %%  Simulate in time
+
 flag_sim = input('Show Simulation? [0/1]');
 if(flag_sim == 1)
     x_state_init = zeros(length(x_state),1); x_state_init = [1 0 0]';
     time_sec = 10; % 10 secs
     time_sec = input('how long [s]?');
     [y,u_ctrl_seq,t] = simulate_Multiparametric_Approximate_ClosedLoop_MinMax(add_usys_d,sol_x_mp,x_state_init,time_sec,MP_SolutionOut.Optimizer_x,W_x_bounds,x_state);
 end
+
 %%  Export and Test Real-Time Controller
 
 ExplicitRobustMPC_obj = Assemble_ExplicitRobustMPC_obj(sol_x_mp,add_usys_d,U_x_bounds,Y_x_Limit_orig)
-%%
+
+%%  Validate
+
 %   1. validator:
 sol_x_mp{1}.Ai = cell(1,length(sol_x_mp{1}.Ai));
 sol_x_mp{1}.Bi = sol_x_mp{1}.Fi;
@@ -126,7 +110,7 @@
 %   derive the matrices for real-time execution
 [H,K,F,G,Nc] = GetRobustMPC_Matrices(sol_x_mp{1});
 Nx = length(add_usys_d.matrices.A);
-%%
+
 %   validate
 clc
 xx = [0.08151 0.089 0.0009]';
@@ -137,7 +121,7 @@
 assign(x_state,xx);
 double(Optimizer)
 
-%%
+%%  Write
 TTR_RobustMPC.Xdir = ExplicitRobustMPC_obj;
 TTR_RobustMPC.Ydir = ExplicitRobustMPC_obj; 
 TTR_RobustMPC.Zdir = ExplicitRobustMPC_obj;