車輪型倒立振子の計算　その２

動いてはいるが、ゲインが大きすぎる
以下の本を参考にした

clear; close all; clc;

A = [0 1 0
    131.91 -1.27E-04 50.27
    -168.29 1.63E-04 -136.55];
B = [0;-56.15;152.52];
C = [1 0 0];
D = 0;
states = {'theta','theta_dot','phi_dot'};
inputs = {'u'};
outputs = {'theta'};

sys = ss(A,B,C,D,'statename',states,'inputname',inputs,'outputname',outputs);
poles = eig(A)

Ae = [A zeros(3,1)
     -C zeros(1,1)];
Be = [B; 0];
Q11 = 1E12; Q22 = 15;
Re = 10;
Qe = [C'*Q11*C zeros(3,1)
     zeros(1,3) Q22];
Pe = care(Ae,Be,Qe,Re);
P11 = Pe(1:3,1:3);
P12 = Pe(1:3,4);
P22 = Pe(4,4);

K = -inv(Re)*B'*P11
G = -inv(Re)*B'*P12

M0 = [A B
      C 0];
Fa = [-K+2*G*inv(P22)*P12' 1]*inv(M0)*[zeros(3,1); 1]
Fb = -2*G*inv(P22)*P12';

Final value 0.3へのステップ応答

入力が10万ボルトとかになっている