clear
format compact

I1=1/12*0.005*(0.03*0.03);
I2=(1/12*0.0125*(0.11*0.11))+(1/4*0.0125*(0.11/2*0.11/2));
I3=(1/12*0.0103*(0.09*0.09))+(1/4*0.0103*(0.09/2*0.09/2));
m1=0.00279*3*2*0.1*3;
m2=(0.00279*pi*0.3*0.3*11)+(0.00279*pi*0.2*0.2*11);
m3=(0.00279*pi*0.3*0.3*9)+(0.00279*pi*0.2*0.2*9);

L2=0.11;
L3=0.09;

l2=0.11/2;
l3=0.09/2;
C1=0.01;
C2=0.001;
C3=0.001;
g=9.8;

% M行列のこと
D=[I1 0 0;
    0 I2+I3+m2*l2^2+m3*L2^2+2*m3*L2*l3+m3*l3^2 I3+m3*L2*l3+m3*l3^2;
    0 I3+m3*L2*l3+m3*l3^2 I3+m3*l3^2];

C=[C1 0 0;0 C2 0;0 0 C3];

G=[0 0 0;
    0 m2*g*l2+m3*g*L2+m3*g*l3 m3*g*l3;
    0 m3*g*l3 m3*g*l3];

% M行列の逆行列
X=inv(D);
% M行列の行列式計算
Y=det(D);

% 状態方程式でのA行列の左下要素のこと
M1=-X*G;
% 状態方程式でのA行列の右下要素のこと
M2=-X*C;

% A行列作成
A=[zeros(3) eye(3)
   M1 M2 ];
%A=[0 0 0 1 0 0;
%    0 0 0 0 1 0;
%    0 0 0 0 0 1;
%    0 0 0 -2.6667*10^4 0 0;
%    0 -102.4426 65.8778 0 -0.0009*10^4 0.0024*10^4;
%    0 123.0226 -304.7424 0 0.0024*10^4 -0.0091*10^4];

% B行列作成
B=[zeros(3)
    X];
%B=[0 0 0;
%    0 0 0;
%    0 0 0;
%    2.6667*10^6 0 0;
%    0 0.0095*10^6 -0.024*10^6;
%    0 -0.024*10^6 0.0914*10^6];

% 重みQ, Rを試行錯誤で決定
Q=diag([3300 3300 3300 0 0 0]); 
R=diag([1 1 1]);
    
%CC=[1 0 0 1 0 0;
%    0 1 0 0 1 0;
%    0 0 1 0 0 1];

% ゲイン行列が求まる！！
K=lqr(A,B,Q,R)

% 可制御行列の作成
Uc = ctrb(A,B);

%Ucc = ctrbf(A,B,CC);

% 可制御性の検証
rank(Uc)
%%det(Uc)
%rank(Ucc)
%det(Ucc)
