172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
|
## Scale R
R = R*dt; # Scale to give correct units
for i = 1:m_t
Ust = Us(i,:);
J_uu = J_uu + Ust'*R*Ust;
endfor
## 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);
|
<
<
<
<
<
<
|
172
173
174
175
176
177
178
179
180
181
182
183
184
185
|
## Scale R
R = R*dt; # Scale to give correct units
for i = 1:m_t
Ust = Us(i,:);
J_uu = J_uu + Ust'*R*Ust;
endfor
## 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);
|
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
|
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);
## Compute the closed-loop gains
k_x = Us0*K_x;
k_w = Us0*K_w;
endfunction
|
>
>
>
>
>
>
>
|
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
|
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
## 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
|