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

まだ途中

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;
sys = ss(A,B,C,D);
Re = 1;
Qe = [1 0 0 0;
      0 1 0 0 
      0 0 1 0
      0 0 0 1];
N = 0;
[K S e] = lqi(sys,Qe,Re);
K1 = K(1:3);
K2 = K(4);

Ae = [A-B*K1 -B*K2
     -C 0];
Be = [B;1];
Ce = [C 0];
De = D;
sys2 = ss(Ae,Be,Ce,De);
step(sys2);