Overview
Comment:Changed timing - now works with inverted pendulum.
Downloads: Tarball | ZIP archive | SQL archive
Timelines: family | ancestors | descendants | both | origin/master | trunk
Files: files | file ages | folders
SHA3-256: d896096f57e602c07fd774328061e5c58aca5e39de19ec9c0b126118c6e85f2c
User & Date: gawthrop@users.sourceforge.net on 2004-10-14 21:42:44
Other Links: branch diff | manifest | tags
Context
2004-10-20
21:58:12
Updated for liuping's qp_hild.m
Used on IP experiment
check-in: efb6c42d89 user: gawthrop@users.sourceforge.net tags: origin/master, trunk
2004-10-14
21:42:44
Changed timing - now works with inverted pendulum. check-in: d896096f57 user: gawthrop@users.sourceforge.net tags: origin/master, trunk
21:40:36
Monor changes - preparing for Inverted pendulum work check-in: 6069fc9eef user: gawthrop@users.sourceforge.net tags: origin/master, trunk
Changes

Modified mttroot/mtt/lib/control/PPP/ppp_lin_run.m from [e3761c925e] to [2ee57392e6].

11
12
13
14
15
16
17


18
19
20
21
22
23
24
  ## Control = 0:  step test
  ## Control = 1:  PPP open-loop
  ## Control = 2:  PPP closed-loop
  ## w is the (constant) setpoint
  ## par_control and par_observer are structures containing parameters
  ## for the observer and controller



  ##Defaults
  if nargin<1			# Default name to dir name
    names = split(pwd,"/");
    [n_name,m_name] = size(names);
    Name = deblank(names(n_name,:));
  endif








>
>







11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
  ## Control = 0:  step test
  ## Control = 1:  PPP open-loop
  ## 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,:));
  endif

123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
	  p_c.A_u = ppp_aug(0,p_c.A_u);
	endif
      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];
    else
      error(sprintf("Method %s not recognised", p_c.Method));







|







125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
	  p_c.A_u = ppp_aug(0,p_c.A_u);
	endif
      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];
    else
      error(sprintf("Method %s not recognised", p_c.Method));
159
160
161
162
163
164
165

166
167
168
169
170
171
172
  endif
  
  ## Initialise
  x_est = p_o.x_0;

  ## Initialise simulation state
  x = 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);
    K_w(1,1) = 1;
    K_w(2,1) = -1;







>







161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
  endif
  
  ## 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);
    K_w(1,1) = 1;
    K_w(2,1) = -1;
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
  
  ## Initial control U
  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
  
  if strcmp(p_o.method, "intermittent")
    Ad = expm(A*p_c.delta_ol);		# Discrete-time transition matrix







<







202
203
204
205
206
207
208

209
210
211
212
213
214
215
  
  ## Initial control U
  U = zeros(p_c.n_U,1)	;

  ## Short sample interval
  dt = p_c.delta_ol/p_c.N;


  ## 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
  
  if strcmp(p_o.method, "intermittent")
    Ad = expm(A*p_c.delta_ol);		# Discrete-time transition matrix
262
263
264
265
266
267
268

269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
  tick = time;
  i=0;

  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!!
      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
	endif
      endif







>

|
<
<
<
<
<
<
<







264
265
266
267
268
269
270
271
272
273







274
275
276
277
278
279
280
  tick = time;
  i=0;

  for j=1:p_c.Iterations
    for k=1:I
      tim=time;			# Timing
      i++;

      if Simulate		# Exact simulation 
	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
	endif
      endif
295
296
297
298
299
300
301
302
303
304
305
306
307
308

309








310
311
312
313
314
315
316
317
318
	  Ui = A_ud'*Ui;
	  y_e = [y_e; y_new'];
	  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;
	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
      
      ##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 
	[Gamma_u, gamma_u] = \
	    ppp_input_constraints(p_c.A_u,p_c.Tau_u,p_c.Min_u,p_c.Max_u);







|
|
|




>
|
>
>
>
>
>
>
>
>

|







291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
	  Ui = A_ud'*Ui;
	  y_e = [y_e; y_new'];
	  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;
	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 
	[Gamma_u, gamma_u] = \
	    ppp_input_constraints(p_c.A_u,p_c.Tau_u,p_c.Min_u,p_c.Max_u);
326
327
328
329
330
331
332



333
334
335
336
337
338
339
	Gamma = [Gamma_u; Gamma_y];
	gamma = [gamma_u; gamma_y];
	
	[u_qp,U,n_active] = ppp_qp \
	    (x_est,w,J_uu,J_ux,J_uw,Us0,Gamma,gamma,1e-6,1);
      endif




      ## Save data
      if Simulate
	t = [t;ti'];
	y = [y;yi'];
	X_est = [X_est;x_est'];
	y_c = [y_c;(C_c*xsi)'];
	u = [u;ui'];







>
>
>







331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
	Gamma = [Gamma_u; Gamma_y];
	gamma = [gamma_u; gamma_y];
	
	[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'];
	y_c = [y_c;(C_c*xsi)'];
	u = [u;ui'];


MTT: Model Transformation Tools
GitHub | SourceHut | Sourceforge | Fossil RSS ]