離散系リカッティ方程式

• 1行と16行: 関数の始まり。
• 3行，19行，20行: 局所変数の宣言。
• 6行，8行，10行，23行: 定数行列の入力。
• 9行，26行，27行: 行列の計算。
• 12行: 関数DRiccati()を呼ぶ。
• 13行: 解行列Pを表示する。
• 17行: 関数の引数の宣言。
• 26行: 行列P - PPのフロベニウスノルムの大きさを調べる。
• 28行: 離散時間系のリカッティ方程式の計算
• 29行: 行列Pの対称性を回復。

   Func void main()
   {
     Matrix A, B, C, Q, R, P, PP;
     Matrix DRiccati();
05:     
     A = [[0 1][-3, 4]];       // システム行列
     B = [0 1]';
     C = [-2, 1];
     Q = C'*C;
10:  R = I(1);
   
     P = DRiccati(A, B, Q, R); // DRiccati()を呼ぶ
     print P;                  // Pを表示
   }
15:
   Func Matrix DRiccati(A, B, Q, R)
     Matrix A, B, Q, R;
   {
     Matrix P, PP;
20:  Real eps;
   
     P = PP = I(A);          // 解 P の初期値
     eps = 1.0E-7;
   
25:  // 解が収束するまで繰り返す
     while (frobnorm(P - PP) > eps) {
         PP = P;
         P = Q + A'*P*A - A'*P*B*(R + B'*P*B)~ * B'*P*A;
         P = (P' + P)/2.0;   // P の対称性を回復
30:  }
   
     return P;               // 解 P を返す
   }