40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
|
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,:);
[n_x, n_u, n_y] = abcddim(A,B,C,D); # Dimensions
[n_x, n_u, n_y_c] = abcddim(A,B,C_c,D_c); # Dimensions
if nargin<2
Simulate = 1;
|
|
|
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
|
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,:);
[n_x, n_u, n_y] = abcddim(A,B,C,D); # Dimensions
[n_x, n_u, n_y_c] = abcddim(A,B,C_c,D_c); # Dimensions
if nargin<2
Simulate = 1;
|
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
|
if !struct_contains(p_o,"method")
##p_o.method = "continuous";
##p_o.method = "intermittent";
p_o.method = "remote";
endif
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;
K_x = zeros(p_c.n_U,n_x);
else
I = ceil(p_c.T/p_c.delta_ol) # Number of large samples
if strcmp(p_c.Method, "original")
[k_x,k_w,K_x,K_w,Us0,J_uu,J_ux,J_uw,J_xx,J_xw,J_ww] =\
ppp_lin(A,B,C,D,p_c.A_u,p_c.A_w,p_c.tau); # Design
elseif strcmp(p_c.Method, "lq") # LQ design
[k_x,k_w,K_x,K_w,Us0,J_uu,J_ux,J_uw,J_xx,J_xw,J_ww,A_u] \
= ppp_lin_quad (A,B,C,D,p_c.tau,p_c.Q,p_c.R,p_c.A_e);
p_c.A_u = A_u
else
error(sprintf("Control method %s not recognised", p_c.Method));
endif
##Sanity check A_u
[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
|
<
|
|
|
|
|
|
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
|
if !struct_contains(p_o,"method")
##p_o.method = "continuous";
##p_o.method = "intermittent";
p_o.method = "remote";
endif
## Check w.
[n_w,m_w] = size(w);
if ( (n_w!=n_y) || (m_w!=1) )
error(sprintf("ppp_lin_run: w must a column vector with %i elements",n_y));
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;
K_x = zeros(p_c.n_U,n_x);
else
I = ceil(p_c.T/p_c.delta_ol) # Number of large samples
if strcmp(p_c.Method, "original")
[k_x,k_w,K_x,K_w,Us0,J_uu,J_ux,J_uw,J_xx,J_xw,J_ww] =\
ppp_lin(A,B,C,D,p_c.A_u,p_c.A_w,p_c.tau); # Design
elseif strcmp(p_c.Method, "lq") # LQ design
[k_x,k_w,K_x,K_w,Us0,J_uu,J_ux,J_uw,J_xx,J_xw,J_ww,A_u] \
= ppp_lin_quad (A,B,C,D,p_c.tau,p_c.Q,p_c.R,p_c.A_e);
p_c.A_u = A_u;
else
error(sprintf("Control method %s not recognised", p_c.Method));
endif
##Sanity check A_u
[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
|
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
|
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);
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
|
|
|
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
|
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
|