車輪型倒立振子の制御 その３ LQI制御

定常偏差は０になるのだが、制御入力uが無限大に発散してしまう

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)

R = 1;
Q = C'*C;
K=lqr(A,B,Q,R);

 Q = blkdiag(1,1,1,1E7);
 R = 0.1;
 
 K=lqi(sys,Q,R)
 K1 = K(1:3);
 K2 = K(4);

 Ae = [A-B*K1 -B*K2
       -C      0];
 Be = [0;0;0;1];
 Ce = [1 0 0 0];
 De = 0;
 
 sys_cl = ss(Ae,Be,Ce,De);
 t = 0:0.01:3;
 r = 0.1*ones(length(t),1)';
 [y,t,x]=lsim(sys_cl,r,t);
 u = -K*x';
 yyaxis left
 
 plot(t,y);hold on;
 yyaxis right
 plot(t,u');
 legend;
 dcgain(sys_cl)
big