 function [Ac,Bc,Cc,Dc,phaiD0] = hinf6a(g,pp,A,B1,B2,C1,C2,D11,D12,D21,D22)
% Loop Shifting を用いた Hinf 制御器の設計
% g:gamma pp:youlaのparameter
% phaiD0 :厳密にproper化するppの値

[n,nn] = size(A); 
% 可制御性、可観測性について
RANKAB2=rank(ctrb(A,B2)); 
if RANKAB2 < n
 disp('uncontrolable')
end
RANKAC2=rank(obsv(A,C2));
if RANKAC2 < n 
 disp('unobservable')
end 

% 各変数のSizeを求める。

[dz,du] = size(D12);
[dy,dw] = size(D21); 

% rankD12,rankD21の条件
if rank(D12) < du
 disp('rankD12 ?')
end
if rank(D21) < dy
 disp('rankD21 ?')
end

% G12,G21の零点に関する条件（充分）
% (A-B2*inv(D12'*D12)*D12'*C1,D12tyo*C1)が可観測か
[D12tyo,D12p] = chol(eye(dz)-D12*inv(D12'*D12)*D12');
RANKAC1=rank(obsv(A-B2*inv(D12'*D12)*D12'*C1,D12tyo*C1));
%(A-B2*inv(D12'*D12)*D12'*C1)が虚軸上に極を持つか
if RANKAC1 < n 
 if min(abs(real(eig(A-B2*inv(D12'*D12)*D12'*C1)))) == 0 
   disp('G12 zero!')
 end 
end 
% (A-B1*D21'*inv(D21*D21')*C2,B1*D21tyo)が可制御か
[D21tyot,D21p] = chol(eye(dw)-D21'*inv(D21*D21')*D21);
D21tyo = D21tyot';
RANKAB1=rank(ctrb(A-B1*D21'*inv(D21*D21')*C2,B1*D21tyo)); 
%(A-B1*D21'*inv(D21*D21')*C2)が虚軸上に極を持つか
if RANKAB1 < n
 if min(abs(real(eig(A-B1*D21'*inv(D21*D21')*C2)))) == 0 
  disp('G21 zero!')
 end
end

% D12 D21 の特異値分解

[U1d,S1d,V1] = svd(D12);
S1 = S1d(1:rank(D12),1:rank(D12));
U1 = [U1d(1:dz,rank(D12)+1:dz) U1d(1:dz,1:rank(D12))];

[U2,S2d,V2d] = svd(D21);
S2 = S2d(1:rank(D21),1:rank(D21));
V2dt = V2d';
V2t = [V2dt(rank(D21)+1:dw,1:dw)
       V2dt(1:rank(D21),1:dw)    ];
V2 = V2t';

% Glover Doyle 標準系への変換

B1d = B1*V2;
B2d = B2*V1*inv(S1);
C1d = U1'*C1;
C2d = inv(S2)*U2'*C2;
D11d = U1'*D11*V2;
D12d = U1'*D12*inv(S1)*V1';
D21d = inv(S2)*U2'*D21*inv(V2');

% Hinf問題を解くための定義

D1122 = D11d(dz-du+1:dz,dw-dy+1:dw);
D1111 = D11d(1:dz-du,1:dw-dy);
D1112 = D11d(1:dz-du,dw-dy+1:dw);
D1121 = D11d(dz-du+1:dz,1:dw-dy);

%特異値の条件
if g <= max([max(svd([D1111 D1112])) max(svd([D1111' D1121']))])
disp('gamma <= tokuiti')
end

% []の処理
if D1111 == []
D1111 =0;
end
if D1112 == []
D1112 =0;
end
if D1121 == []
D1121 =0;
end
%
D1_ = [D11d D12d];
D_1 = [D11d
       D21d];

gg1 = [ g^2*eye(dw)   zeros([dw,du])
       zeros([du,dw]) zeros([du,du])];
R = D1_'*D1_ - gg1;
gg2 = [ g^2*eye(dz)   zeros([dz,dy])
       zeros([dy,dz]) zeros([dy,dy])];
J = D_1*D_1'-gg2;
B = [ B1d B2d];
C = [C1d
     C2d];

Ax = A-B*inv(R)*D1_'*C1d;
Qx = eye(dz)-D1_*inv(R)*D1_';
Ay = A-B1d*D_1'*inv(J)*C;
Qy = eye(dw)-D_1'*inv(J)*D_1;

% Riccati方程式
[X1,X2,lampX,perrX,wellX,X] = aresolv(Ax,C1d'*Qx*C1d,B*inv(R)*B');
if wellX == 'FALSE'
disp('wellX = FALSE')
end
if min(real(eig(X)))<0
 disp('X = ? ')
end
if max(real(lampX)) >=0
 disp('lampX >= 0')
end

[Y1,Y2,lampY,perrY,wellY,Y] = aresolv(Ay',B1d*Qy*B1d',C'*inv(J)*C);
if wellY == 'FALSE'
disp('wellY = FALSE')
end
if min(real(eig(Y)))<0
disp(' Y = ? ')
end
if max(real(lampY)) >=0
 disp('lampY >= 0')
end

%スペクトル半径
eigYX = max(abs(eig(Y*X)));
if eigYX >= g^2
disp('eigYX>=g^2')
eigYX
end


D11I = -D1121*D1111'*inv(g^2*eye(dz-du)-D1111*D1111')*D1112-D1122;
[D12It,p] = chol(eye(du)-D1121*inv(g^2*eye(dw-dy)-D1111'*D1111)*D1121');
D12I = (D12It)';
[D21I,q] = chol(eye(dy)-D1112'*inv(g^2*eye(dz-du)-D1111*D1111')*D1112);

F = -inv(R)*(D1_'*C1d+B'*X);
F11 = F(1:dw-dy,1:n);
F12 = F(dw-dy+1:dw,1:n);
F2 = F(dw+1:dw+du,1:n);

L = -(B1d*D_1'+Y*C')*inv(J);
L11 = L(1:n,1:dz-du);
L12 = L(1:n,dz-du+1:dz);
L2 = L(1:n,dz+1:dz+dy);

Z = inv(eye(n)-g^(-2)*Y*X);
B2I = (B2d+L12)*D12I;
C2I = -D21I*(C2d+F12)*Z;
B1I = -L2+B2I*inv(D12I)*D11I;
C1I = F2*Z + D11I*inv(D21I)*C2I;
AI = A+L*C+B2I*inv(D12I)*C1I;


Ah = AI+B2I*pp*C2I;
Bh = (B1I+B2I*pp*D21I)*inv(S2)*U2';
Ch = V1*inv(S1)*(C1I+D12I*pp*C2I);
Dh = V1*inv(S1)*(D11I+D12I*pp*D21I)*inv(S2)*U2';

% D22に対する変換

Ac = Ah - Bh*D22*inv(eye(du)+Dh*D22)*Ch;
Bc = Bh-Bh*D22*inv(eye(du)+Dh*D22)*Dh;
Cc = inv(eye(du)+Dh*D22)*Ch;
Dc = inv(eye(du)+Dh*D22)*Dh;

% Youlaの自由paramater
phaiD0 = -inv(D12I)*D11I*inv(D21I);

