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
209
|
## 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
## 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;
## w bits
if n_W>0
for i = 1:n_y # For each output
QQ = ones(n_U,1)*Q(i,:); # Resize Q
J_uw = J_uw + (QQ .* y_u(:,i:n_y:n_yu)') * Ws (:,i:n_y:n_yw);
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
## 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);
|
<
<
<
<
<
<
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
|
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
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
|
## 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
## w bits
if n_W>0
for i = 1:n_y # For each output
QQ = ones(n_U,1)*Q(i,:); # Resize Q
J_uw = J_uw + (QQ .* y_u(:,i:n_y:n_yu)') * Ws (:,i:n_y:n_yw);
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;
## 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);
|