% パラメータの設定
global M m1 m2 l1 l2 g J1 J2
M = 0.455;
m1 = 0.21;
m2 = 0.21;
l1 = 0.61/2;
l2 = 0.61/2;
g = 9.8;
J1 = m1*l1*l1/3;
J2 = m2*l2*l2/3;

% 状態方程式の係数行列の計算
M0 = [M+m1+m2, (m1+2*m2)*l1, m2*l2;
       (m1+2*m2)*l1, J1+(m1+4*m2)*l1^2, 2*m2*l1*l2;
       m2*l2, 2*m2*l1*l2, J2+m2*l2^2];
A0 = [0, 0, 0; 0, (m1+2*m2)*g*l1, 0; 0, 0, m2*g*l2];
B0 = [1; 0; 0];

A = [zeros(3,3), eye(3); M0\A0, zeros(3,3)];
B = [zeros(3,1); M0\B0];

% 状態フィードバックゲインの設定
%F = poleplace(A, B, [-10, -10, -10, -7, -1.2+2i, -1.2-2i]);
%F = poleplace(A, B, [-10, -10, -2+3.2i, -2-3.2i, -1.2+3i, -1.2-3i]);
%F = poleplace(A, B, [-100, -13.5, -2.66+3.1i, -2.66-3.1i, -2.66+3.1i, -2.66-3.1i]);
F = poleplace(A, B, 5*[-1, -1, -1, -1, -1, -1]);

% 閉ループ系の固有値を表示
eig(A + B*F)
