状態フィードバック制御のシミュレーション (1)

関数main()はパラメータと初期状態を設定して シミュレーションを行ない，結果をファイルcont.matに保存する。 diff_eqs()は動的要素の微分方程式を記述するための関数であり， link_eqs()は動的要素の接続関係を記述するための関数である。

関数Ode45Auto()とOde45HybridAuto()は陽の非線形関数の積分を行なう。 関数Ode45Auto()は連続時間制御のシュミレーションを行ない， 関数Ode45HybridAuto()はサンプル値系(ディジタルコントローラ による連続時間プラントの制御)のシュミレーションを行なう。

関数Ode45Auto()は，5個の引数をとる。 引数は，初期時刻(0.0)，最終時刻 (5.0)，初期状態ベクトル(x0)， 微分係数ベクトル(dx)を計算する関数の参照(diff_eqs)， 入力ベクトル(u)を計算する関数の参照(link_eqs)の5個である。 関数Ode45HybridAuto()は，上述の5個の引数に加えて， 一定入力(u)がプラントに与え続けられるサンプリング時間(dt)を 引数にとる。

微分係数ベクトルを計算する関数diff_eqs()と入力ベクトルを計算する関数 link_eqs()は，特別なフォーマットでなければならない。 関数diff_eqs()は4個の引数を持つ。 第1引数(dx)は，微分係数ベクトルを返すために使われ，状態ベクトル (x)と同じ次元である。第2引数(t)は，現時刻を表す実数である。 第3引数(x)は，現時刻の状態ベクトルであり，第4引数(u)は， 動的要素への入力ベクトルである。 関数link_eqs()は3個の引数を持つ。 第1引数(u)は，入力ベクトルを返すために使われる。 第2引数(t)は，現時刻を表す実数である。 第3引数(x)は，現時刻の状態ベクトルである。    

Func void main()
{
  Real dt;
  Matrix x0;
  Array TC, XC, UC, TH, XH, UH;
  void diff_eqs(), link_eqs();

  dt = 0.01;       // サンプリング時間
  x0 = [1 1 1]';   // 初期状態
  {TC,XC,UC} = Ode45Auto(0.0, 5.0, x0, diff_eqs, link_eqs);
  x0 = [1 1 1]';   // 初期状態
  {TH,XH,UH} = 
     Ode45HybridAuto(0.0, 5.0, dt, x0, diff_eqs, link_eqs);

  print [[TC][XC][UC]] >> "cont.mat";
  print [[TH][XH][UH]] >> "hybd.mat";
}

Func void diff_eqs(dx, t, x, u)
  Real t;
  Matrix x, dx, u;
{
  Matrix A, b;

  A = [[ 0   1   0]      // システム行列
       [ 0   0   1]
       [-2, -3, -4]];

  b = [0 0 1]';          // 入力行列
  dx = A*x + b*u;        // 状態方程式
}

Func void link_eqs(u, t, x)
  Real t;
  Matrix u, x;
{
  Matrix f;

  f = [-1, -1, -1];   // 状態フィードバックゲイン
  u = f*x;
}