Index: mttroot/mtt/lib/control/PPP/ppp_lin_run.m ================================================================== --- mttroot/mtt/lib/control/PPP/ppp_lin_run.m +++ mttroot/mtt/lib/control/PPP/ppp_lin_run.m @@ -13,10 +13,12 @@ ## Control = 2: PPP closed-loop ## w is the (constant) setpoint ## par_control and par_observer are structures containing parameters ## for the observer and controller + disp("Special version"); + ##Defaults if nargin<1 # Default name to dir name names = split(pwd,"/"); [n_name,m_name] = size(names); Name = deblank(names(n_name,:)); @@ -125,11 +127,11 @@ endif else error(sprintf("Method %s not recognised", p_c.Method)); endif endif - + if !struct_contains(p_c,"tau") # Time horizon if strcmp(p_c.Method,"lq") p_c.tau = [0:0.1:1]*2; elseif strcmp(p_c.Method,"original"); p_c.tau = [10:0.1:11]; @@ -161,10 +163,11 @@ ## Initialise x_est = p_o.x_0; ## Initialise simulation state x = x_0; + y_i = C*x_0 if ControlType==0 # Step input I = 1; # 1 large sample p_c.delta_ol = p_c.T; # I K_w = zeros(p_c.n_U,n_y); @@ -201,11 +204,10 @@ U = zeros(p_c.n_U,1) ; ## Short sample interval dt = p_c.delta_ol/p_c.N; -p_o ## Observer design G = eye(n_x); # State noise gain sigma_x = eye(n_x); # State noise variance Sigma = p_o.sigma*eye(n_y) # Measurement noise variance @@ -264,19 +266,13 @@ for j=1:p_c.Iterations for k=1:I tim=time; # Timing i++; + if Simulate # Exact simulation - X = x; # Current state - t_sim = [1:p_c.N]*dt; # Simulation time points - [yi,ui,xsi] = ppp_ystar(A,B,C,D,x,p_c.A_u,U,t_sim); # Simulate - x = xsi(:,p_c.N); # Current state (for next time) - ti = [(i-1)*p_c.N:i*p_c.N-1]*dt; - y_i = yi(1); # Current output - t_i = ti(1); - ##X = xsi(:,1); # Wrong!! + X = x; # Current (simulated) state else # The real thing if strcmp(p_o.method, "remote") [t_i,y_i,u_i,X] = ppp_put_get_X(U); # Remote-state interface else [t_i,y_i,u_i] = ppp_put_get(U); # Generic interface to real-time @@ -297,20 +293,29 @@ e_e = [e_e; e_est']; endfor elseif strcmp(p_o.method, "remote") ## predict from remote state (with zero L) if (ControlType==2) # Closed-loop -# [x_est y_est y_new e_est] = ppp_int_obs \ -# (X,y_i,U,A,B,C,D,p_c.A_u,p_c.delta_ol,zeros(n_x,1)); - x_est = X; y_est=y_i; y_new=y_i; e_est=0; + [x_est y_est y_new e_est] = ppp_int_obs \ + (X,y_i,U,A,B,C,D,p_c.A_u,p_c.delta_ol,zeros(n_x,1)); + ## x_est = X; y_est=y_i; y_new=y_i; e_est=0; else # Open-loop [x_est y_est y_new e_est] = ppp_int_obs \ (x_est,y_i,U,A,B,C,D,p_c.A_u,p_c.delta_ol,zeros(n_x,1)); endif - endif + ## Simulation (based on U_i) + if Simulate + t_sim = [1:p_c.N]*dt; # Simulation time points + [yi,ui,xsi] = ppp_ystar(A,B,C,D,x,p_c.A_u,U,t_sim); # Simulate + x = xsi(:,p_c.N); # NEXT state + ti = [(i-1)*p_c.N:i*p_c.N-1]*dt; + y_i = yi(1); # Current output + t_i = ti(1); + endif + ##Control if ( length(p_c.Tau_u)==0&&length(p_c.Tau_y)==0 ) U = K_w*w - K_x*x_est; else ## Input constraints @@ -328,10 +333,13 @@ [u_qp,U,n_active] = ppp_qp \ (x_est,w,J_uu,J_ux,J_uw,Us0,Gamma,gamma,1e-6,1); endif + ## Allow for the delay + ##U = expm(p_c.delta_ol*p_c.A_u)*U; + ## Save data if Simulate t = [t;ti']; y = [y;yi']; X_est = [X_est;x_est'];