function [T,y,u,Iterations] = ppp_qp_sim (A,B,C,D,A_u,A_w,t,Q, Tau_u,Min_u,Max_u,Order_u, Tau_y,Min_y,Max_y,Order_y, W,x_0,Delta_ol,mu,movie) ## usage: [T,y,u,J] = ppp_qp_sim (A,B,C,D,A_u,A_w,t,Q, Tau_u,Min_u,Max_u,Order_u, Tau_y,Min_y,Max_y,Order_y, W,x_0,movie) ## Needs documentation - see ppp_ex11 for example of use. ## OUTPUTS ## T: Time vector ## y,u,J output, input and cost ## Copyright (C) 1999 by Peter J. Gawthrop ## $Id$ if nargin<19 # No intermittent control Delta_ol = 0; endif if nargin<20 # No movie mu = 0; endif if nargin<21 # No movie movie = 0; endif ## Check some sizes [n_x,n_u,n_y] = abcddim(A,B,C,D); [n_x0,m_x0] = size(x_0); if (n_x0 != n_x)||(m_x0 != 1) error(sprintf("Initial state x_0 must be %ix1 not %ix%i",n_x,n_x0,m_x0)); endif ## Input constraints (assume same on all inputs) Gamma_u=[]; gamma_u=[]; for i=1:n_u [Gamma_i,gamma_i] = ppp_input_constraint (A_u,Tau_u,Min_u,Max_u,Order_u,i,n_u); Gamma_u = [Gamma_u; Gamma_i]; gamma_u = [gamma_u; gamma_i]; endfor ## Output constraints [Gamma_y,gamma_y] = ppp_output_constraint (A,B,C,D,x_0,A_u,Tau_y,Min_y,Max_y,Order_y); ## Composite constraints - t=0 Gamma = [Gamma_u; Gamma_y]; gamma = [gamma_u; gamma_y]; ## Design the controller disp("Designing controller"); [k_x,k_w,K_x,K_w,Us0,J_uu,J_ux,J_uw,J_xx,J_xw,J_ww] = ppp_lin (A,B,C,D,A_u,A_w,t,Q); ## Set up various time vectors dt = t(2)-t(1); # Time increment ## Make sure Delta_ol is multiple of dt Delta_ol = floor(Delta_ol/dt)*dt; if Delta_ol>0 # Intermittent control T_ol = 0:dt:Delta_ol-dt; # Create the open-loop time vector else T_ol = 0; Delta_ol = dt; endif T_cl = 0:Delta_ol:t(length(t))-Delta_ol; # Closed-loop time vector n_Tcl = length(T_cl); Ustar_ol = ppp_ustar(A_u,n_u,T_ol); # U* in the open-loop interval [n,m] = size(Ustar_ol); n_U = m/length(T_ol); # Determine size of each Ustar ## Discrete-time system csys = ss2sys(A,B,C,D); dsys = c2d(csys,dt); [Ad, Bd] = sys2ss(dsys); x = x_0; # Initialise state ## Initialise the saved variable arrays X = []; u = []; Iterations = []; du = []; J = []; tick= time; i = 0; disp("Simulating ..."); for t=T_cl # Outer loop at Delta_ol ##disp(sprintf("Time %g", t)); ## Output constraints [Gamma_y,gamma_y] = ppp_output_constraint (A,B,C,D,x,A_u,Tau_y,Min_y,Max_y,Order_y); ## Composite constraints Gamma = [Gamma_u; Gamma_y]; gamma = [gamma_u; gamma_y]; ## Compute U(t) [uu, U, iterations] = ppp_qp (x,W,J_uu,J_ux,J_uw,Us0,Gamma,gamma,mu); # Compute U ## Compute the cost (not necessary but maybe interesting) # [J_t] = ppp_cost (U,x,W,J_uu,J_ux,J_uw,J_xx,J_xw,J_ww); # cost # J = [J J_t]; ## Simulation loop i_ol = 0; for t_ol=T_ol # Inner loop at dt ## Compute ol control i_ol = i_ol+1; range = (i_ol-1)*n_U + 1:i_ol*n_U; # Extract current U* ut = Ustar_ol(:,range)*U; # Compute OL control (U* U) ## Simulate the system i = i+1; X = [X x]; # Save state u = [u ut]; # Save input Iterations = [Iterations iterations]; # Save iteration count x = Ad*x + Bd*ut; # System # if movie # Plot the moving horizon # tau = T(1:n_T-i); # Tau with moving horizon # tauT = T(i+1:n_T); # Tau with moving horizon + real time # [ys,us,xs,xu,AA] = ppp_ystar (A,B,C,D,x,A_u,U,tau); # OL response # plot(tauT,ys, tauT(1), ys(1), "*") # endif endfor endfor ## Save the last values X = [X x]; # Save state u = [u ut]; # Save input Iterations = [Iterations iterations]; # Save iteration count tock = time; Elapsed_Time = tock-tick y = C*X + D*u; # System output T = 0:dt:t+Delta_ol; # Overall time vector endfunction