Ltitr.java

/*
 * $Id: Ltitr.java,v 1.13 2008/03/08 00:17:41 koga Exp $
 *
 * Copyright (C) 2004 Koga Laboratory. All rights reserved.
 */
package org.mklab.tool.control;

import org.mklab.nfc.matrix.DoubleMatrix;


/**
 * 離散時間線形システムの任意入力に対する応答を求めるクラスです。
 * 
 * <p>Time response of discrete-time linear system
 * 
 * @author koga
 * @version $Revision: 1.13 $
 * @see org.mklab.tool.control.Ltifr
 */
public class Ltitr {

  /**
   * 離散時間線形システム
   * 
   * <pre><code> x[n + 1] = Ax[n] + Bu[n] </code></pre>
   * 
   * に入力の系列 <code>U</code> が加えられた場合の時間応答を計算します。 行列 <code>U</code> の各列が各時刻の入力です。
   * 
   * @param A A行列
   * @param B B行列
   * @param U 入力の列
   * @return 状態の応答 (state response)
   */
  public static DoubleMatrix ltitr(DoubleMatrix A, DoubleMatrix B, DoubleMatrix U) {
    return ltitr(A, B, U, A.createZero(A.getColumnSize(), 1));
  }

  /**
   * 初期状態を <code>x0</code> とします。
   * 
   * @param A A行列
   * @param B B行列
   * @param inputSequence 入力の列
   * @param x0 初期状態
   * @return 状態の応答 (response)
   */
  public static DoubleMatrix ltitr(DoubleMatrix A, DoubleMatrix B, DoubleMatrix inputSequence, DoubleMatrix x0) {
    DoubleMatrix U = inputSequence;
    String message;
    if ((message = Abcdchk.abcdchk(A, B)).length() > 0) {
      throw new RuntimeException(message);
    }

    if (U.getRowSize() > U.getColumnSize()) {
      U = U.transpose();
    }

    int n = A.getColumnSize();
    int N = U.length();

    DoubleMatrix X = A.createZero(n, N);
    DoubleMatrix x = x0;
    for (int i = 1; i <= N; i++) {
      X.setSubMatrix(1, X.getRowSize(), i, i, x);
      DoubleMatrix u = U.getColumnVector(i);
      x = A.multiply(x).add(B.multiply(u));
    }

    if (U.getRowSize() > U.getColumnSize()) {
      return X.transpose();
    }
    return X;
  }
}