60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
|
endif
if !struct_contains(p_c,"augment")
p_c.augment = 0; # Augment basis funs with constant
endif
if !struct_contains(p_c,"integrate")
p_c.integrate = 0; # Augment basis funs with constant
endif
if !struct_contains(p_c,"Tau_u")
p_c.Tau_u = [];
p_c.Min_u = [];
p_c.Max_u = [];
endif
|
|
|
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
|
endif
if !struct_contains(p_c,"augment")
p_c.augment = 0; # Augment basis funs with constant
endif
if !struct_contains(p_c,"integrate")
p_c.integrate = 0;
endif
if !struct_contains(p_c,"Tau_u")
p_c.Tau_u = [];
p_c.Min_u = [];
p_c.Max_u = [];
endif
|
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
|
elseif strcmp(p_c.Method,"original");
p_c.tau = [10:0.1:11];
else
error(sprintf("Method %s not recognised", p_c.Method));
endif
endif
if !struct_contains(p_o,"x_0")
p_o.x_0 = zeros(n_x,1);
endif
if !struct_contains(p_o,"method")
##p_o.method = "continuous";
|
|
>
>
|
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
|
elseif strcmp(p_c.Method,"original");
p_c.tau = [10:0.1:11];
else
error(sprintf("Method %s not recognised", p_c.Method));
endif
endif
if !struct_contains(p_c,"A_e")
p_c.A_e = []; # No extra modes
endif
if !struct_contains(p_o,"x_0")
p_o.x_0 = zeros(n_x,1);
endif
if !struct_contains(p_o,"method")
##p_o.method = "continuous";
|
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
|
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")
## tau = [10:0.1:11]*(2/a_u); # Time horizons
## tau = [0:0.001:1]*2; # Time horizons
= ppp_lin_quad (A,B,C,D,p_c.tau,p_c.Q,p_c.R);
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)
|
<
<
|
|
|
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
|
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)
|
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
|
ppp_output_constraints(A,B,C,D,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
|
>
|
<
|
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
|
ppp_output_constraints(A,B,C,D,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'];
u = [u;ui'];
else
|