倒立振子の制御のシミュレーション

Real a32, a33, a34, a35, a42, a43, a44, a45;
Real b3, b4, alpha;
Matrix C, F, Ah, Bh, Ch, Dh, Jh, Xo;

Func void main()
{
  Real t0, tf, dt;
  Matrix TC, XC, UC, x0;
  void para_init(), diff_eqs(), link_eqs();

  para_init();                 // パラメータの初期化
  t0 = 0.0; tf = 3.0; dt = 0.005;

  x0 = [0 30.0/180.0*PI 0 0]'; // 初期状態
  Xo = Z(2,1);                 // 推定値の初期値

  {TC, XC, UC} = 
    Ode45HybridAuto(t0, tf, dt, x0, diff_eqs, link_eqs);
}

次に，状態方程式を記述する関数diff_eqs()を示す。  

Func void diff_eqs(DX, t, X, U)
  Matrix DX, X, U;
  Real t;
{
  Real x1, x2, x3, x4, u, c2, s2, dm;

  x1 = X(1); // 台車の位置
  x2 = X(2); // 振子の角度
  x3 = X(3); // 台車の速度(推定値)
  x4 = X(4); // 振子の角速度(推定値)
  u = U(1);  // 入力

  c2 = cos(x2); s2 = sin(x2);
  dm = (1 + alpha*s2^2);

  DX = Z(4,1);  // 状態方程式
  DX(1) = X(3);
  DX(2) = X(4);
  DX(3) = (a32*s2*c2 + a33*x3 + a34*c2*x4  
           + a35*s2*x4^2 + b3*u)/dm;
  DX(4) = (a42*s2 + a43*c2*x3 + a44*x4 
           + a45*s2*c2*x4^2 + b4*c2*u)/dm;
}

Func void link_eqs(U, t, X)
 Matrix U, X;
 Real t;
{
 Matrix Y, Xh;

 Y  = C * X;               // 出力方程式
 Xh = Ch*Xo + Dh*Y;        // オブザーバの推定
 U  = - F*Xh;              // 状態フィードバック
 Xo = Ah*Xo + Bh*Y + Jh*U; // オブザーバの更新
}