44
45
46
47
48
49
50
51
52
53
54
55
56
57
|
if nargin<9
R = zeros(n_u,1);
endif
## Default P (terminal weight)
if nargin<10
P = zeros(n_x,n_x);
endif
## Default condittion number
if nargin<11
max_cond = 1e20;
endif
|
>
>
>
|
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
|
if nargin<9
R = zeros(n_u,1);
endif
## Default P (terminal weight)
if nargin<10
P = zeros(n_x,n_x);
P_weighting = 0
else
P_weighting = 1
endif
## Default condittion number
if nargin<11
max_cond = 1e20;
endif
|
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
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
|
QQ = ones(n_x,1)*Q(i,:); # Resize Q
J_xw = J_xw + (QQ .* y_x(:,i:n_y:n_yx)') * Ws (:,i:n_y:n_yw);
QQ = ones(n_W,1)*Q(i,:); # Resize Q
J_ww = J_ww + (QQ .* Ws (:,i:n_y:n_yw)') * Ws (:,i:n_y:n_yw);
endfor
endif
## Terminal constraint
tau_last = tau(m_t);
x_x_t = expm(A*tau_last); # deriv. of x*(tau_2) wrt x(0)
J_ux = J_ux + x_u_t'*P*x_x_t;
J_uu = J_uu + x_u_t'*P*x_u_t;
## Terminal constraint - w bits
## NB SISO ONLY at the moment
C = C(1,:); # REMOVE ME
## deriv of x* wrt W
if (cond(A)<1e10) # Finite ss gain
x_W_t = (A\B)*inv(C*(A\B));
else # Infinite ss gain
Y_0 = zeros(n_x,n_y); Y_0(1,:) = ones(1,n_y);
x_W_t = obsv(A,C)\Y_0;
endif
J_uw = J_uw + x_u_t'*P*x_W_t;
## Exit if badly conditioned
cond_uu = cond(J_uu);
if cond_uu>max_cond
error(sprintf("J_uu is badly conditioned. Condition number = 10^%i",log10(cond_uu)));
endif
|
>
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
>
|
|
>
<
|
187
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
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
|
QQ = ones(n_x,1)*Q(i,:); # Resize Q
J_xw = J_xw + (QQ .* y_x(:,i:n_y:n_yx)') * Ws (:,i:n_y:n_yw);
QQ = ones(n_W,1)*Q(i,:); # Resize Q
J_ww = J_ww + (QQ .* Ws (:,i:n_y:n_yw)') * Ws (:,i:n_y:n_yw);
endfor
endif
if P_weighting
## Terminal constraint
tau_last = tau(m_t);
x_x_t = expm(A*tau_last); # deriv. of x*(tau_2) wrt x(0)
J_ux = J_ux + x_u_t'*P*x_x_t;
J_uu = J_uu + x_u_t'*P*x_u_t;
## Terminal constraint - w bits
## NB SISO ONLY at the moment
C = C(1,:); # REMOVE ME
## deriv of x* wrt W
if (cond(A)<1e10) # Finite ss gain
x_W_t = (A\B)*inv(C*(A\B));
else # Infinite ss gain
Y_0 = zeros(n_x,n_y); Y_0(1,:) = ones(1,n_y);
x_W_t = obsv(A,C)\Y_0;
endif
J_uw = J_uw + x_u_t'*P*x_W_t;
endif
## Exit if badly conditioned
cond_uu = cond(J_uu);
if cond_uu>max_cond
error(sprintf("J_uu is badly conditioned. Condition number = \
10^%i",log10(cond_uu)));
endif
## Compute the open-loop gains
K_w = J_uu\J_uw;
K_x = J_uu\J_ux;
## U*(tau) at tau=0
Us0 = ppp_ustar(A_u,n_u,0);
## Compute the closed-loop gains
k_x = Us0*K_x;
k_w = Us0*K_w;
endfunction
|