h2state.m

hard disk drive servo control tools, it is very helpful

function [k,x] = h2state(a,b,c,d,ep)
%
%      [F,P] = h2state(A,B,C,D,epsilon) returns an H2 suboptimal state
%         feedback control law u = F x for the following continuous-
%         time system:
%              .
%              x = A x + B u + E w
%              y =   x
%              z = C x + D u
%
%         Note that the system need not satisfy regularity assumptions.
 
%       Ben M. Chen,  07-07-90
%       Department of Electrical Engineering and Computer Science
%       Washington State University
%       Pullman, Washington 99164-2752
%

e1=c;e2=d;d=0*b;h=0*c*b;ga=1000000;  
[n,m] = size(b); [n,kk] = size(d); [j,n] = size(e1);
if rank(e2) == 0
   sumh = e2'*e2;
   phi = eye(m);
end
if rank( e2 )>0
   [u,s,v] = svd(e2); i = rank(s);
   u = u*s; u = u(:,1:i); v = v(:,1:i)';
   phi = null(v)';
   r = eye(j)+h*inv(ga^2*eye(kk)-h'*h)*h';
   sumh = v'*inv(v*v')*inv(u'*r*u)*inv(v*v')*v;
end
in = inv(ga^2*eye(kk)-h'*h);
stupid = rand(j); stupid = stupid*stupid';
Sqrin = sqrtm(eye(j)+h*in*h'+ stupid*1e-13);
Ah = a+d*in*h'*e1;
Bh = b+d*in*h'*e2;
Ch = Sqrin*e1;
stupid = rand(kk); stupid = stupid*stupid';
Dh = d*sqrtm(in+ stupid*1e-13);
Fh = Sqrin*e2;
A = Ah-Bh*sumh*Fh'*Ch;
R = Bh*sumh*Bh'-Dh*Dh';
if max( size(phi) )>0
   R = R+1/ep*Bh*phi'*phi*Bh';
end
Q = Ch'*(eye(j)-Fh*sumh*Fh')*Ch+ep*eye(n);
x = are(A,R,Q);
k = sumh*Bh'*x+sumh*Fh'*Ch;
if max( size(phi) ) > 0
k = k+1/ep*phi'*phi*Bh'*x;
k = -real(k);     % due to adding stupid in this program.
end
%
% - - - - - - - - - - - - BMC - - - 90M05 - - - - - - - - - - - - - - -