clear
format compact

% --- 既知パラメータ ----------------------------
% L1 = 7.50e-02;
L1 = 7.10e-02;
m2 = 4.00e-03;
l2 = 7.10e-02;
g  = 9.81e+00;
% --- 同定されたアームのパラメータ（二次遅れ）---
a     = 1.42e+01;
b     = 3.45e+00;
% --- 同定された振子のパラメータ（最小二乗法）-----
J2 = 9.27e-06;
c2 = 1.33e-05;

% --------------------------------------------------------
J2b = J2 + m2*l2^2;

A = [ 0  0            1                0
      0  0            0                1
	  0  0           -a                0
	  0  m2*g*l2/J2b  m2*L1*l2*a/J2b  -c2/J2b ]
B = [ 0
      0
      b
     -m2*L1*l2*b/J2b ]
% --------------------------------------------------------
q1 = 5e1;
q2 = 2e3;
q3 = 1e-2;
q4 = 1e-2;

Q  = diag([q1 q2 q3 q4]);  
R  = 1;

K = - lqr(A,B,Q,R)
eig(A + B*K)

h   = 0.01;

Tf1 = 0.06;
Tf2 = 0.06;

