207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
|
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
|
-
+
-
+
-
-
+
+
-
+
|
t_i = ti(1);
else # The real thing
[t_i,y_i,u_i] = ppp_put_get(U); # Generic interface to real-time
endif
## Observer
if strcmp(p_o.method, "intermittent")
[x_est y_est e_est] = ppp_int_obs \
[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 e_est] = ppp_int_obs \
[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_est'];
e_e = [e_e; e_est];
y_e = [y_e; y_new'];
e_e = [e_e; e_est'];
endfor
endif
##Control
U = K_w*w - K_x*x_est;
## Save data
if Simulate
t = [t;ti'];
y = [y;yi'];
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_est'];
y_e = [y_e; y_new'];
e_e = [e_e; e_est'];
t_e = [t_e; t_i];
endif
delta_comp = time-tim;
usleep(floor(1e6*(p_c.delta_ol-delta_comp-0.01)));
endfor # Main loop
|