車輪型倒立振子の状態方程式

ここを参考に
倒立2輪ロボットの安定化制御
https://www2.akita-nct.ac.jp/kizawa/paper/kiyo2011_sato.pdf

clear; close all; clc;

m = 0.281; %kg;
M = 0.102; %kg;
L = 0.037; %m;
r = 0.029; %m;
I = 1.63E-04;%kg•m2
J = 2.357E-05; %kg•m2
Dphi = 2.00E-07; %kg•m/s2
Dth = 1.00E-04; %kg•m/s2
g = 9.8; %m/s2

n = 3;%gear ratio
KT = 0.0163; %0.0163[N•m/A]=16.3[mN•m/A];
KE = KT;
Jm = 91.9; %91.9[kg•m2]=9.19E-03[kg•cm2]=9.19[g•cm2]
Ra = 1.34; %ohm

a = (m+M)*r^2+J;
b = M*r*L;
c = M*L^2+I;
d = M*g*L;

E = a + Jm*n^2;
F = a+b;
G= Dphi+KT*KE*n^2/Ra;
H = a+2*b+c;

a21 = E*d/(F^2-E*H);
a22 = E*Dth/(F^2-E*H);
a23 = F*G/(F^2-E*H);
a31 = F*d/(F^2-E*H);
a32 = F*Dth/(F^2-E*H);
a33 = G*H/(F^2-E*H);
b2 = E*KT*n/(Ra*(F^2-E*H));
b3 = -H*KT*n/(Ra*(F^2-E*H));

A = [0 1 0; a21 a22 a23; a31 a32 a33];
B = [0; b2; b3]; 
C = [1 0 0];
D = 0;
sys = ss(A,B,C,D);

G = tf(sys);