Dinitial.java

/*
 * $Id: Dinitial.java,v 1.28 2008/03/24 23:45:43 koga Exp $
 *
 * Copyright (C) 2004 Koga Laboratory. All rights reserved.
 */
package org.mklab.tool.control;

import java.io.IOException;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;

import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.matrix.DoubleRationalPolynomialMatrix;
import org.mklab.nfc.scalar.DoubleRationalPolynomial;
import org.mklab.tool.graph.gnuplot.Canvas;
import org.mklab.tool.graph.gnuplot.Gnuplot;


/**
 * 離散時線形システムの初期値応答を求めるクラスです。
 * 
 * <p>Initial response of discrete-time linear system
 * 
 * @author koga
 * @version $Revision: 1.28 $
 * @see org.mklab.tool.control.Dimpulse
 * @see org.mklab.tool.control.Dstep
 * @see org.mklab.tool.control.Initial
 */
public class Dinitial {

  /**
   * 離散時間システム
   * 
   * <pre><code>
   * 
   * x[n+1] = Ax[n] + Bu[n] y[n] = Cx[n] + Du[n]
   * 
   * </code></pre>
   * 
   * の初期値応答を計算します。
   * 
   * @param A A行列
   * @param B B行列
   * @param C C行列
   * @param D D行列
   * @param x0 初期状態
   * @param T 評価する時刻(等間隔の時刻) (regularly spaced time vector)
   * @return {Y, X} (出力と状態の応答) response
   */
  public static List<DoubleMatrix> dinitial(DoubleMatrix A, DoubleMatrix B, DoubleMatrix C, DoubleMatrix D, DoubleMatrix x0, DoubleMatrix T) {
    String message;
    if ((message = Abcdchk.abcdchk(A, B, C, D)).length() > 0) {
      throw new IllegalArgumentException(message);
    }

    int stateSize = A.getColumnSize();
    int N = T.length();

    DoubleMatrix X = A.createZero(stateSize, N);

    DoubleMatrix x = x0;
    for (int i = 1; i <= N; i++) {
      X.setSubMatrix(1, X.getRowSize(), i, i, x);
      x = A.multiply(x);
    }

    DoubleMatrix Y = C.multiply(X);

    return new ArrayList<>(Arrays.asList(new DoubleMatrix[] {Y, X}));
    //return new MatxList(new Object[] {Y, X});
  }

  /**
   * @param g 伝達関数
   * @param x0 初期状態
   * @param T 評価す時刻
   * @return {Y, X} (出力と状態の応答) response
   */
  public static List<DoubleMatrix> dinitial(DoubleRationalPolynomial g, DoubleMatrix x0, DoubleMatrix T) {
    List<DoubleMatrix> abcd = Tfn2ss.tfn2ss(g);
    DoubleMatrix A = abcd.get(0);
    DoubleMatrix B = abcd.get(1);
    DoubleMatrix C = abcd.get(2);
    DoubleMatrix D = abcd.get(3);
    return dinitial(A, B, C, D, x0, T);
  }

  /**
   * @param G 伝達関数行列
   * @param x0 初期状態
   * @param T 評価する時刻
   * @return {Y, X} (出力と状態の応答) response
   */
  public static List<DoubleMatrix> dinitial(DoubleRationalPolynomialMatrix G, DoubleMatrix x0, DoubleMatrix T) {
    List<DoubleMatrix> abcd = Tfm2ss.tfm2ss(G, 0);
    DoubleMatrix A = abcd.get(0);
    DoubleMatrix B = abcd.get(1);
    DoubleMatrix C = abcd.get(2);
    DoubleMatrix D = abcd.get(3);
    return dinitial(A, B, C, D, x0, T);
  }

  /**
   * @param numerator 伝達関数の分子多項式の係数
   * @param denominator 伝達関数の分母多項式の係数
   * @param x0 初期状態
   * @param T 評価する時刻
   * @return {Y, X} (出力と状態の応答) response
   */
  public static List<DoubleMatrix> dinitial(DoubleMatrix numerator, DoubleMatrix denominator, DoubleMatrix x0, DoubleMatrix T) {
    List<DoubleMatrix> abcd = Tf2ss.tf2ss(numerator, denominator);
    DoubleMatrix A = abcd.get(0);
    DoubleMatrix B = abcd.get(1);
    DoubleMatrix C = abcd.get(2);
    DoubleMatrix D = abcd.get(3);
    return dinitial(A, B, C, D, x0, T);
  }

  /**
   * @param g 伝達関数
   * @param x0 初期状態
   * @param T 評価する時刻
   * @return Gnuplot
   * @throws IOException gnuplotプロセスを起動できない場合
   */
  public static Gnuplot plot(DoubleRationalPolynomial g, DoubleMatrix x0, DoubleMatrix T) throws IOException {
    Gnuplot gp = new Gnuplot();
    return plot(gp, g, x0, T);
  }

  /**
   * @param gnuplot gnuplot
   * @param g 伝達関数
   * @param x0 初期状態
   * @param T 評価する時刻
   * @return Gnuplot
   */
  @SuppressWarnings("nls")
  public static Gnuplot plot(Gnuplot gnuplot, DoubleRationalPolynomial g, DoubleMatrix x0, DoubleMatrix T) {
    List<DoubleMatrix> yx = dinitial(g, x0, T);
    DoubleMatrix Y = yx.get(0);
    Canvas canvas = gnuplot.createCanvas();
    canvas.setGridVisible(true);
    canvas.setXLabel("t [sec]");
    canvas.setYLabel("y");
    canvas.plot(T, Y, new String[] {"y"});
    return gnuplot;
  }

  /**
   * @param numerator 分子多項式の係数
   * @param denominator 分母多項式の係数
   * @param x0 初期状態
   * @param T 評価する時刻
   * @return Gnuplot
   * @throws IOException gnuplotプロセスを起動できない場合
   */
  public static Gnuplot plot(DoubleMatrix numerator, DoubleMatrix denominator, DoubleMatrix x0, DoubleMatrix T) throws IOException {
    Gnuplot gp = new Gnuplot();
    return plot(gp, numerator, denominator, x0, T);
  }

  /**
   * @param gnuplot gnuplot
   * @param numerator 分子多項式の係数
   * @param denominator 分母多項式の係数
   * @param x0 初期状態
   * @param T 評価する時刻
   * @return Gnuplot
   */
  public static Gnuplot plot(Gnuplot gnuplot, DoubleMatrix numerator, DoubleMatrix denominator, DoubleMatrix x0, DoubleMatrix T) {
    DoubleRationalPolynomial g = Tf2tfn.tf2tfn(numerator, denominator);
    return plot(gnuplot, g, x0, T);
  }

  /**
   * @param G 伝達関数行列
   * @param x0 初期状態
   * @param T 評価する時刻
   * @return Gnuplot
   * @throws IOException gnuplotプロセスを起動できない場合
   */
  public static Gnuplot plot(DoubleRationalPolynomialMatrix G, DoubleMatrix x0, DoubleMatrix T) throws IOException {
    Gnuplot gp = new Gnuplot();
    return plot(gp, G, x0, T);
  }

  /**
   * @param gnuplot gnuplot
   * @param G 伝達関数行列
   * @param x0 初期状態
   * @param T 評価する時刻
   * @return Gnuplot
   */
  @SuppressWarnings("nls")
  public static Gnuplot plot(Gnuplot gnuplot, DoubleRationalPolynomialMatrix G, DoubleMatrix x0, DoubleMatrix T) {
    List<DoubleMatrix> yx = dinitial(G, x0, T);
    DoubleMatrix Y = yx.get(0);
    int outputSize = G.getRowSize();
    String[] yy = new String[outputSize];
    for (int i = 1; i <= outputSize; i++) {
      if (outputSize == 1) {
        yy[i - 1] = "y";
      } else {
        yy[i - 1] = "y" + i;
      }
    }
    Canvas canvas = gnuplot.createCanvas();
    canvas.setGridVisible(true);
    canvas.setXLabel("t [sec]");
    canvas.setYLabel("y");
    canvas.plot(T, Y, yy);
    return gnuplot;
  }

  /**
   * @param A A行列
   * @param B B行列
   * @param C C行列
   * @param D D行列
   * @param x0 初期状態
   * @param T 評価する時刻
   * @return Gnuplot
   * @throws IOException gnuplotプロセスを起動できない場合
   */
  public static Gnuplot plot(DoubleMatrix A, DoubleMatrix B, DoubleMatrix C, DoubleMatrix D, DoubleMatrix x0, DoubleMatrix T) throws IOException {
    Gnuplot gp = new Gnuplot();
    return plot(gp, A, B, C, D, x0, T);
  }

  /**
   * @param gnuplot gnuplot
   * @param A A行列
   * @param B B行列
   * @param C C行列
   * @param D D行列
   * @param x0 初期状態
   * @param T 評価する時刻
   * @return Gnuplot
   */
  @SuppressWarnings("nls")
  public static Gnuplot plot(Gnuplot gnuplot, DoubleMatrix A, DoubleMatrix B, DoubleMatrix C, DoubleMatrix D, DoubleMatrix x0, DoubleMatrix T) {
    List<DoubleMatrix> yx = dinitial(A, B, C, D, x0, T);
    DoubleMatrix Y = yx.get(0);
    DoubleMatrix X = yx.get(1);

    int outputSize = C.getRowSize();
    String[] yy = new String[outputSize];
    for (int i = 1; i <= outputSize; i++) {
      if (outputSize == 1) {
        yy[i - 1] = "y";
      } else {
        yy[i - 1] = "y" + i;
      }
    }

    gnuplot.createCanvas(2, 1);
    Canvas canvas1 = gnuplot.getCanvas(0, 0);
    canvas1.setGridVisible(true);
    canvas1.setXLabel("t [sec]");
    canvas1.setYLabel("y");
    canvas1.plot(T, Y, yy);

    int stateSize = A.getRowSize();
    String[] xx = new String[stateSize];
    for (int i = 1; i <= stateSize; i++) {
      xx[i - 1] = "x" + i;
    }

    Canvas canvas2 = gnuplot.getCanvas(1, 0);
    canvas2.setGridVisible(true);
    canvas2.setXLabel("t [sec]");
    canvas2.setYLabel("x");
    canvas2.plot(T, X, xx);
    return gnuplot;
  }

}