倒立振子のリアルタイム制御

Func void on_task()
{
  Matrix Y, Xh;

  Y  = sensor();            // センサから入力
  Xh = Ch*Xo + Dh*Y;        // オブザーバの推定
  U  = - F*Xh;              // 状態フィードバック
  actuator(U);              // アクチュエータへ出力
  Xo = Ah*Xo + Bh*Y + Jh*U; // オブザーバの更新
}