倒立振子 (制御器)




Func void link_eqs(UY, t, x)
  Matrix UY, x;
  Real t;
{
  Matrix u, y, xh;
  extern Matrix z;

  y  = C*x;                  // 出力方程式
  xh = Chd*z + Dhd*y;        // 状態推定
  u  = - F*xh;               // 状態フィードバック
  z = Ahd*z + Bhd*y + Jhd*u; // 観測器状態更新
  UY = [[u][y]];
}

(詳しい式は論文参照)