︙ | | | ︙ | |
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'];
|
︙ | | | ︙ | |