%% サイズ設定

n = 4;
Nm0 = 14;
Nm1 = 1;
Nm2 = 1;
Np0 = 14;
Np1 = 5;

%% 物理パラメータ

% L1 = 0.127;
L1 = 0.13;
% L2 = 0.207;
L2 = 0.2;
%m1 = 0.216;
m1 = 0.22;
%m2 = 2.11e-2;
m2 = 2.1e-2;
% J1 = 2.89e-4;
J1 = 3e-4;
% J2 = 1.39e-4;
J2 = 1.4e-4;
%l1 = 0.1173;
l1 = 0.12;
% l2 = 9.8e-2;
l2 = 0.1;
% c1 = 1.07e-4;
c1 = 1.0e-4;
% c2 = 5.04e-4;
c2 = 5.0e-4;
g = 9.8;

alpha2 = J2 + m2*l2^2;
alpha3 = m2*L1*l2;
alpha5 = m2*l2*g;

a = 100;  % 根拠なし
b = 100;  % 根拠なし
norm_dq1 = 10;

% mu0 = pi/3;
mu0 = pi/12;
mu_c = mu0/(mu0^2+9)*(mu0*cos(mu0)-3*sin(mu0)+2*mu0);
mu_s = (3*cos(mu0)-9*sin(mu0)/mu0+6)/(mu0^2+9);
mu_d = mu0;

%% ディスクリプタ形式 係数行列計算

E = [
  1, 0, 0, 0;
  0, 1, 0, 0;
  0, 0, 1, 0;
  0, 0, alpha2, alpha2
];

F = zeros(n,Np0);
F(4,2) = 1;
F(4,6) = -alpha5;

Ab = [
  0, 0, 1, 0;
  0, 0, 0, 1;
  0, 0, -a, 0;
  0, 0, 0, -c2
];

B0b = zeros(n,Nm0);
B0b(4,1) = mu0/2;
B0b(4,5) = -alpha5*mu0/2;
B0b(4,9) = -mu_c;
B0b(4,11) = alpha5*mu_c;
B0b(4,13) = -alpha3*mu_d;

B1b = [ 0; 0; 0; 1 ];

B2b = [ 0; 0; b; 0 ];

Gamma = zeros(Np0,n);
Gamma(2,3) = -alpha3;

Omega = eye(Np0);
Omega(9,2) = -1;
Omega(11,6) = -1;
Omega(14,6) = -1;

C0b = zeros(Np0,n);
C0b(2,1) = -alpha5;
C0b(4,2) = 1;
C0b(6,2) = 1;
C0b(8,1) = 1;
C0b(10,2) = 1;
C0b(12,1) = 1;

D00b = zeros(Np0,Nm0);
D00b(1,2) = mu0;
D00b(3,4) = mu0;
D00b(2,7) = alpha5*mu0/6;
D00b(2,12) = -alpha5*mu_s;
D00b(5,6) = mu0;
D00b(7,8) = mu0;
D00b(6,3) = -mu0/6;
D00b(6,10) = mu_s;
D00b(13,14) = mu_d;

D01b = zeros(Np0,1);

D02b = zeros(Np0,1);

%% 状態空間実現 係数行列計算

Mabcd = [ E, F; Gamma, Omega ]\[ Ab, B0b, B1b, B2b; C0b, D00b, D01b, D02b ];

Ah  = Mabcd(1:n,1:n);
Bh0 = Mabcd(1:n,n+1:n+Nm0);
Bh1 = Mabcd(1:n,n+Nm0+1:n+Nm0+Nm1); 
Bh2 = Mabcd(1:n,n+Nm0+Nm1+1:end); 

Ch0  = Mabcd(n+1:end,1:n);
Dh00 = Mabcd(n+1:end,n+1:n+Nm0);
Dh01 = Mabcd(n+1:end,n+Nm0+1:n+Nm0+Nm1); 
Dh02 = Mabcd(n+1:end,n+Nm0+Nm1+1:end); 

Ch1 = [ eye(n,n); zeros(1,n) ];
Dh10 = zeros(Np1,Nm0);
Dh11 = zeros(Np1,Nm1);
Dh12 = [ zeros(Np1-1,Nm2); 1 ];

Bh = [Bh0, Bh1, Bh2];
Ch = [Ch0; Ch1 ];
Dh = [ 
  Dh00, Dh01, Dh02;
  Dh10, Dh11, Dh12;
];

%% 一般化プラント

Gh = ss(Ah, Bh, Ch, Dh);
m2 = 1;

%% 不確かさの構造

delta_ports = [
  [ 4, 4 ];
  [ 4, 4 ];
  [ 1, 1 ];
  [ 1, 1 ]; 
  [ 1, 1 ]; 
  [ 1, 1 ]; 
  [ 2, 2 ];
];
