%cYMCPB5_of_sm.m
%-----
 clear all, close all
 dataset=[...
 "ABC_No1_20220128",...
 "ABC_No2_20220131",...
 "ABC_No3_20220128",...
 "ABC_No4_20220131",...
 "ABC_No5_20220131"];
 no=1
 load(dataset(no))
 ID=[8,9,10,11,12,13,14,...
 36,37,38,39,40,41,42,43,...
 66,67,68,69,70,71,72,73,...
 97,98,99,100,101,102,103,...
 126,127,128,129,130,131,132,133];
%-----６次元モデル
%(1)x,(2)z,(3)th,(4)dx-V*,(5)dz,(6)dth
 sys=ss(A0(:,:,ID),B0(:,:,ID),eye(6,6),[]);
%-----５次元モデル
%(1)z,(2)th,(3)dz,(4)dth,(5)the
 k=[2,3,5,6]; i=[2,4,5]; j=[2];
 for id=ID
   A(:,:,id)=[A0(k,k,id) B0(k,j,id);zeros(1,5)];
 end 
 omegae=0.0380*3;
 B=[zeros(4,1);omegae];
 E=eye(5,5);
 CM=E(i,:);
 C=CM(3,:);
 sys5=ss(A(:,:,ID),B,eye(5,5),[]);  
%-----設計ポイント id=128=ID(33)
 Vs=dx_eq_Series(128);    %*3.6
 zs=z_eq_Series(128);     %*100
 ths=th_eq_Series(128);   %/pi*180
 thes=the_eq_Series(128); %/pi*180
 [Asys,Bsys,Csys,Dsys]=ssdata(sys(:,:,33));
 [A,B]=ssdata(sys5(:,:,33));
%=====OF-SM
 C=CM;
%Establish the number of inputs and outputs
 [nn,mm]=size(B);
 [pp,nn]=size(C);
%Change coordinates so the output distribution matrix is [0 I]
 nc=null(C);
 Tc=[nc'; C];
 Ac=Tc*A*inv(Tc);
 Bc=Tc*B;
 Cc=C*inv(Tc);
%Partition the input distribution matrix conformably
 Bc1=Bc(1:nn-pp,:);
 Bc2=Bc(nn-pp+1:nn,:);
%Find a transformation to bring about a special structure 
%in input and output distribution matrices
 [T,temp]=qr(Bc2);
 T=(flipud(T'))';
 Tb=[eye(nn-pp) -Bc1*inv(Bc2'*Bc2)*Bc2';
     zeros(pp,nn-pp) T'];
%In this new cordinate system, we have C=[0 T] and B=[0 B2']'
%A has no special structure yet
 Aa=Tb*Ac*inv(Tb);
 Ba=Tb*Bc;
 Ca=Cc*inv(Tb);
 A1111=Aa(1:nn-pp,1:nn-pp);
 A1121=Aa(nn-pp+1:nn-mm,1:nn-pp);
%The aim is to put (A1111,A1121) in the observability canonical form
 [Ab,Bb,Cb,Tobs,k]=obsvf(A1111,zeros(nn-pp,1),A1121,1000*eps);
%r is the dimension of the unobservable subspace 
%the number of invariant zeros of the system (A,B,C)
 r=nn-pp-sum(k);
 disp('Dimension of the unobservable subspace:'),r
 Ta=[Tobs zeros(nn-pp,pp);
     zeros(pp,nn-pp) eye(pp)];
 Af=Ta*Aa*inv(Ta);
 Bf=Ta*Ba;eig(Af)
 Cf=Ca*inv(Ta); 
%=====
 A11ti=Aa(1:nn-mm,1:nn-mm);
 B1ti=Aa(1:nn-mm,nn-mm+1:nn);
 C1ti=[zeros(pp-mm,nn-pp) eye(pp-mm,pp-mm)];
 [Kopt,pl]=opt(A11ti,B1ti,C1ti,diag([1 1]),1)
 K=Kopt/C1ti 
 A11bar=A11ti-B1ti*K*C1ti;
 lambda=eig(A11bar)
%---- 
 Tbar=[eye(nn-mm) zeros(nn-mm,mm);
       K*C1ti eye(mm)]; 
 Abar=Tbar*Aa*inv(Tbar);  
 A11bar2=Abar(1:nn-mm,1:nn-mm);
 lambda=eig(A11bar2) 
 A12bar=Abar(1:nn-mm,nn-mm+1:nn);
 A21bar=Abar(nn-mm+1:nn,1:nn-mm);
 A22bar=Abar(nn-mm+1:nn,nn-mm+1:nn); 
 Q1=eye(nn-mm); 
 P1=lyap(A11bar2',Q1)
% 
 P2=1
 B2=Ba(nn,mm)
 F2=B2'*P2 
 F=F2*[K eye(mm)]*T'
% 
 Q2=P1*A12bar+A21bar'*P2;
 Q3=P2*A22bar+A22bar'*P2; 
 W=inv(F2)'*(Q3+Q2'*inv(Q1)*Q2)*inv(F2);
 gamma0=0.5*norm(W) 
 gamma=gamma0*1.1
 rho=1
%-----
 us=thes;
 xs=[0;zs;ths;Vs;0;0];
 x0=[0;0;-1/180*pi;0;0;0];
 yc=0; 
 E=eye(6,6);
%(1)x,(2)z,(3)th,(4)dx-V*,(5)dz,(6)dth 
 CM=E([3,6],:);
 CM1=100*E([2],:);
 CM2=180/pi*E([3],:);
 ns=2
%-----
%eof