Overview
Comment:Monor changes - preparing for Inverted pendulum work
Downloads: Tarball | ZIP archive | SQL archive
Timelines: family | ancestors | descendants | both | origin/master | trunk
Files: files | file ages | folders
SHA3-256: b64a234faebb04f7f22fdb2ec479e4d9ddff2423d164dd5dfac54abd658ffbf0
User & Date: gawthrop@users.sourceforge.net on 2004-10-14 21:40:36
Other Links: branch diff | manifest | tags
Context
2004-10-14
21:42:44
Changed timing - now works with inverted pendulum. check-in: c3fad309c5 user: gawthrop@users.sourceforge.net tags: origin/master, trunk
21:40:36
Monor changes - preparing for Inverted pendulum work check-in: b64a234fae user: gawthrop@users.sourceforge.net tags: origin/master, trunk
2004-09-14
19:04:35
Improved diagram. check-in: 3984e7174d user: geraint@users.sourceforge.net tags: origin/master, trunk
Changes

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

1
2
3
4
5
6
7
8
9
10
function [t,y,u,y_c,t_e,y_e,e_e] = ppp_lin_run (Name,Simulate,ControlType,w,x_0,p_c,p_o)

  ## usage: [t,y,u,t_e,y_e,e_e] = ppp_lin_run (Name,Simulate,ControlType,w,x_0,p_c,p_o)
  ##
  ## 
  ## Linear closed-loop PPP of lego system (and simulation)
  ##
  ## Name: Name of system (in mtt terms)
  ## Simulate = 0: real thing
  ## Simulate = 1: simulate
|

|







1
2
3
4
5
6
7
8
9
10
function [t,y,u,X_est,y_c,t_e,y_e,e_e,p_c,p_o] = ppp_lin_run (Name,Simulate,ControlType,w,x_0,p_c,p_o)

  ## usage: [t,y,u,y_c,t_e,y_e,e_e,p_c,p_o] = ppp_lin_run (Name,Simulate,ControlType,w,x_0,p_c,p_o)
  ##
  ## 
  ## Linear closed-loop PPP of lego system (and simulation)
  ##
  ## Name: Name of system (in mtt terms)
  ## Simulate = 0: real thing
  ## Simulate = 1: simulate
29
30
31
32
33
34
35

36
37
38
39
40
41
42
43
44
45
46
47
48
  if nargin<7
    p_o.sigma = 1e-1;
  endif

  ## System
  sys = mtt2sys(Name);		# Create system
  [A,B,C_0,D_0] = sys2ss(sys);	# SS form


  ## Extract matrices for controlled and constrained outputs.
  if !struct_contains(p_c,"I_0") # Indices for controlled outputs
    p_c.I_0 = 1:n_y
  endif
  if !struct_contains(p_c,"I_1") # Indices for constarined outputs
    p_c.I_1 = 1:n_y
  endif

  C = C_0(p_c.I_0,:)
  C_c = C_0(p_c.I_1,:);
  D = D_0(p_c.I_0,:);
  D_c = D_0(p_c.I_1,:);







>





|







29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
  if nargin<7
    p_o.sigma = 1e-1;
  endif

  ## System
  sys = mtt2sys(Name);		# Create system
  [A,B,C_0,D_0] = sys2ss(sys);	# SS form
  [n_x, n_u, n_y] = abcddim(A,B,C_0,D_0);

  ## Extract matrices for controlled and constrained outputs.
  if !struct_contains(p_c,"I_0") # Indices for controlled outputs
    p_c.I_0 = 1:n_y
  endif
  if !struct_contains(p_c,"I_1") # Indices for constrained outputs
    p_c.I_1 = 1:n_y
  endif

  C = C_0(p_c.I_0,:)
  C_c = C_0(p_c.I_1,:);
  D = D_0(p_c.I_0,:);
  D_c = D_0(p_c.I_1,:);
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
    w = ones(n_y,1);;
  endif
  
  if nargin<5
    x_0 = zeros(n_x,1);
  endif
  

  if !struct_contains(p_c,"delta_ol")
    p_c.delta_ol = 0.5;	# OL sample interval
  endif
  
  if !struct_contains(p_c,"T")
    p_c.T = 10;			# Last time point.
  endif







<







63
64
65
66
67
68
69

70
71
72
73
74
75
76
    w = ones(n_y,1);;
  endif
  
  if nargin<5
    x_0 = zeros(n_x,1);
  endif
  

  if !struct_contains(p_c,"delta_ol")
    p_c.delta_ol = 0.5;	# OL sample interval
  endif
  
  if !struct_contains(p_c,"T")
    p_c.T = 10;			# Last time point.
  endif
188
189
190
191
192
193
194

195
196
197
198
199
200
201
202
203
204

205
206
207
208
209
210
211
    [p_c.n_U,M_u] = size(p_c.A_u);
    if (p_c.n_U!=M_u)
      error("A_u must be square");
    endif

    ## Checks
    cl_poles = eig(A - B*k_x)

    t_max = 1/min(abs(cl_poles));
    t_min = 1/max(abs(cl_poles));
  endif
  
  ## 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







>










>







188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
    [p_c.n_U,M_u] = size(p_c.A_u);
    if (p_c.n_U!=M_u)
      error("A_u must be square");
    endif

    ## Checks
    cl_poles = eig(A - B*k_x)
    ol_poles = eig(A)
    t_max = 1/min(abs(cl_poles));
    t_min = 1/max(abs(cl_poles));
  endif
  
  ## 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
239
240
241
242
243
244
245
246
247

248
249
250
251
252
253
254
255

256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
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
324
325
326

327
328
329
330
331

332
333
334
335
336
337
338
339
340
341
342
  overrun = 2;
  Ustar = ppp_ustar (p_c.A_u, n_u, [0:dt:overrun*p_c.delta_ol], 0,0);
  if p_c.integrate		# Integrate Ustar
    disp("Integrating Ustar");
    Ustar = cumsum(Ustar)*dt;
  endif
  
  disp("Writing Ustar.h");
  ppp_ustar2h(Ustar); 



  ## Control loop
  y = [];
  y_c = [];
  u = [];
  t = [];
  y_e = [];

  t_e = [];
  e_e = [];
  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,X] = ppp_put_get_X(U); # Remote-state interface
	  u_i = X(3);		# Integrated control is third state
	else
	  [t_i,y_i,u_i] = ppp_put_get(U); # Generic interface to real-time
	endif
      endif

      ## Observer
      if strcmp(p_o.method, "intermittent")
	[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,L);
      elseif strcmp(p_o.method, "continuous")
	Ui = U;			# U at sub intervals
	for k = 1:p_c.N
	  [x_est y_est y_new e_est] = ppp_int_obs \
	      (x_est,yi(:,k),Ui,A,B,C,D,p_c.A_u,dt,L);
	  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)





	[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,L);


      endif
      
      ##Control
      if ( (p_c.Tau_u==[])&&(p_c.Tau_y==[]) )
	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);
	
	## Output constraints
	[Gamma_y,gamma_y] = \
	    ppp_output_constraints(A,B,C_c,D_c,x_est,p_c.A_u,\
				   p_c.Tau_y,p_c.Min_y,p_c.Max_y);
	
	## Composite constraints - t=0
	Gamma = [Gamma_u; Gamma_y];
	gamma = [gamma_u; gamma_y];
	
	[u_qp,U] = 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'];

	y_c = [y_c;(C_c*xsi)'];
	u = [u;ui'];
      else
	t = [t;t_i];
	y = [y;y_i'];

	u = [u;u_i'];
      endif

      if strcmp(p_o.method, "intermittent")
	y_e = [y_e; y_new'];
	e_e = [e_e; e_est'];
	t_e = [t_e; t_i];
      endif
      if !Simulate
	delta_comp = time-tim;
	usleep(floor(1e6*(p_c.delta_ol-delta_comp-0.01)));







|

>








>




















|
<




|














|
>
>
>
>
>
|
|
>
>



|















|







>





>



|







241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280

281
282
283
284
285
286
287
288
289
290
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
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
  overrun = 2;
  Ustar = ppp_ustar (p_c.A_u, n_u, [0:dt:overrun*p_c.delta_ol], 0,0);
  if p_c.integrate		# Integrate Ustar
    disp("Integrating Ustar");
    Ustar = cumsum(Ustar)*dt;
  endif
  
  disp("Writing Ustar.h ...");
  ppp_ustar2h(Ustar); 
  disp("done.");


  ## Control loop
  y = [];
  y_c = [];
  u = [];
  t = [];
  y_e = [];
  X_est = [];
  t_e = [];
  e_e = [];
  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
      
      ## Observer
      if strcmp(p_o.method, "intermittent")
	[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,L);
      elseif strcmp(p_o.method, "continuous")
	Ui = U;			# U at sub intervals
	for k = 1:p_c.N
	  [x_est y_est y_new e_est] = ppp_int_obs \
	      (x_est,yi(:,k),Ui,A,B,C,D,p_c.A_u,dt,L);
	  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);
	
	## Output constraints
	[Gamma_y,gamma_y] = \
	    ppp_output_constraints(A,B,C_c,D_c,x_est,p_c.A_u,\
				   p_c.Tau_y,p_c.Min_y,p_c.Max_y);
	
	## Composite constraints - t=0
	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'];
      else
	t = [t;t_i];
	y = [y;y_i'];
	X_est = [X_est;x_est'];
	u = [u;u_i'];
      endif

      if strcmp(p_o.method, "intermittent")||strcmp(p_o.method, "remote")
	y_e = [y_e; y_new'];
	e_e = [e_e; e_est'];
	t_e = [t_e; t_i];
      endif
      if !Simulate
	delta_comp = time-tim;
	usleep(floor(1e6*(p_c.delta_ol-delta_comp-0.01)));
351
352
353
354
355
356
357
358
359
360
361
362
363
364

  
  if strcmp(p_o.method, "continuous")
    t_e = t;
  endif
  
  
  sample_interval = (time-tick)/i

  ## Put data on file (so can use for identification)
  filename = sprintf("%s_ident_data.dat",Name);
  eval(sprintf("save -ascii %s t y u",filename));

endfunction







|






363
364
365
366
367
368
369
370
371
372
373
374
375
376

  
  if strcmp(p_o.method, "continuous")
    t_e = t;
  endif
  
  
  average_ol_sample_interval = (time-tick)/i

  ## Put data on file (so can use for identification)
  filename = sprintf("%s_ident_data.dat",Name);
  eval(sprintf("save -ascii %s t y u",filename));

endfunction


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