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: 6069fc9eef54d2f02577543c12b91e8a65a29eaab0cff76f9e23b472b98080dd
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: 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
2004-09-14
19:04:35
Improved diagram. check-in: 1226fc1478 user: geraint@users.sourceforge.net tags: origin/master, trunk
Changes
Hide Diffs Side-by-Side Diffs Ignore Whitespace Patch

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

1

2
3

4
5
6
7
8
9
10


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)
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,t_e,y_e,e_e] = 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

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 constarined outputs
  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

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

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

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");
  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,X] = ppp_put_get_X(U); # Remote-state interface
	  [t_i,y_i,u_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);
	## 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 ( (p_c.Tau_u==[])&&(p_c.Tau_y==[]) )
      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] = ppp_qp \
	[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")
      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

363
364
365
366
367
368
369

370
371
372
373
374
375
376








-
+








  
  if strcmp(p_o.method, "continuous")
    t_e = t;
  endif
  
  
  sample_interval = (time-tick)/i
  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 ]