Ramp.java

/*
 * $Id: Ramp.java,v 1.24 2008/07/17 07:30:03 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.matrix.misc.LinearlySpacedVector;
import org.mklab.nfc.scalar.DoubleNumber;
import org.mklab.nfc.scalar.DoublePolynomial;
import org.mklab.nfc.scalar.DoubleRationalPolynomial;
import org.mklab.nfc.util.Pause;
import org.mklab.tool.graph.gnuplot.Canvas;
import org.mklab.tool.graph.gnuplot.Gnuplot;


/**
 * 連続時間線形システムのランプ応答を求めるクラスです。
 * 
 * <p>Ramp response of continuous-time linear systems
 * 
 * @author koga
 * @version $Revision: 1.24 $
 * @see org.mklab.tool.control.Step
 * @see org.mklab.tool.control.Impulse
 */
public class Ramp {

  /**
   * メインメソッド
   * 
   * @param args コマンドライン引数
   * @throws IOException キーボードから入力できない場合
   */
  public static void main(String[] args) throws IOException {
    DoublePolynomial s = new DoublePolynomial("s"); //$NON-NLS-1$
    DoubleRationalPolynomial g1 = new DoubleRationalPolynomial(1);
    DoubleRationalPolynomial g2 = new DoubleRationalPolynomial(new DoublePolynomial(1), s.add(1));
    DoubleRationalPolynomial g = g1.multiply(g2);
    g.print();
    DoubleMatrix T = LinearlySpacedVector.create(0, 2, 100);
    Gnuplot gp = plot(g, T);
    try {
      Pause.pause();
    } finally {
      gp.close();
    }
  }

  /**
   * 連続時間システム
   * 
   * <pre><code>
   * 
   * . x = Ax + Bu y = Cx + Du
   * 
   * </code></pre>
   * 
   * の <code>inputNumber</code> 番目の入力に単位ランプ入力が加えられた 場合のランプ応答を求めます。
   * 
   * <p>もし <code>inputNumber = 0</code>なら、 <pre><code>
   * 
   * [[Y for 1st input] [[X for 1st input] [Y for 2nd input] [X for 2nd input] [...............] [...............] [Y for m'th input]] と [Y for m'th input]].
   * 
   * </code></pre> を返します。
   * 
   * @param A A行列
   * @param B B行列
   * @param C C行列
   * @param D D行列
   * @param inputNumber 入力番号
   * @param T 時刻の列(等間隔)
   * @return {YY, XX} (出力と状態の応答) response
   */
  public static List<DoubleMatrix> ramp(DoubleMatrix A, DoubleMatrix B, DoubleMatrix C, DoubleMatrix D, int inputNumber, DoubleMatrix T) {
    String message;
    if ((message = Abcdchk.abcdchk(A, B, C, D)).length() > 0) {
      throw new RuntimeException(message);
    }

    DoubleMatrix XX, YY;

    DoubleNumber unit = A.getElement(1, 1);

    // integrator
    DoubleMatrix aI = unit.createGrid(2, 2, new DoubleNumber[][] { {unit.createZero(), unit.clone()}, {unit.createZero(), unit.createZero()}});
    DoubleMatrix bI = unit.createGrid(2, 1, new DoubleNumber[][] { {unit.createZero()}, {unit.createUnit()}});
    DoubleMatrix cI = unit.createGrid(1, 2, new DoubleNumber[][] {{unit.createUnit(), unit.createZero()}});
    DoubleMatrix dI = A.createZero(1, 1);

    if (inputNumber == 0) {
      int inputSize = B.getColumnSize();
      int N = T.length();
      YY = A.createZero(C.getRowSize() * inputSize, N);
      XX = A.createZero(A.getRowSize() * inputSize, N);
      for (int i = 1; i <= inputSize; i++) {
        List<DoubleMatrix> tmp = Series.series(aI, bI, cI, dI, A, B.getColumnVector(i), C, D.getColumnVector(i));
        DoubleMatrix A2 = tmp.get(0);
        DoubleMatrix B2 = tmp.get(1);
        DoubleMatrix C2 = tmp.get(2);
        DoubleMatrix D2 = tmp.get(3);

        List<DoubleMatrix> tmp2 = Impulse.impulse(A2, B2, C2, D2, 1, T);
        DoubleMatrix Y = tmp2.get(0);
        DoubleMatrix X = tmp2.get(1);
        X = X.getRowVectors(3, X.getRowSize()); // remove the state of integrator
        YY.setSubMatrix(i, 1, Y, Y);
        XX.setSubMatrix(i, 1, X, X);
      }
    } else {
      List<DoubleMatrix> tmp = Series.series(aI, bI, cI, dI, A, B.getColumnVector(inputNumber), C, D.getColumnVector(inputNumber));

      DoubleMatrix A2 = tmp.get(0);
      DoubleMatrix B2 = tmp.get(1);
      DoubleMatrix C2 = tmp.get(2);
      DoubleMatrix D2 = tmp.get(3);

      List<DoubleMatrix> tmp3 = Impulse.impulse(A2, B2, C2, D2, 1, T);
      YY = tmp3.get(0);
      XX = tmp3.get(1);
      XX = XX.getRowVectors(3, XX.getRowSize()); // remove the state of integrator
    }

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

  /**
   * 単位ランプ応答を計算します。
   * 
   * @param system 線形システム
   * @param T 時刻の列
   * @return {YY, XX} (出力と状態の応答) response
   */
  public static List<DoubleMatrix> ramp(DoubleLinearSystem system, DoubleMatrix T) {
    if (system.isProper()) {
      DoubleMatrix A = system.getA();
      DoubleMatrix B = system.getB();
      DoubleMatrix C = system.getC();
      DoubleMatrix D = system.getD();
      return ramp(A, B, C, D, 1, T);
    }

    return ramp(system.getTransferFunctionMatrix(), 1, T);
  }

  /**
   * 単位ランプ応答を計算します。
   * 
   * @param g 伝達関数
   * @param T 時刻の列
   * @return {YY, XX} (出力と状態の応答) response
   */
  public static List<DoubleMatrix> ramp(DoubleRationalPolynomial g, DoubleMatrix T) {
    List<DoubleMatrix> tmp = Tfn2ss.tfn2ss(g);
    DoubleMatrix A = tmp.get(0);
    DoubleMatrix B = tmp.get(1);
    DoubleMatrix C = tmp.get(2);
    DoubleMatrix D = tmp.get(3);
    return ramp(A, B, C, D, 1, T);
  }

  /**
   * 単位ランプ応答を計算します。
   * 
   * @param G 伝達関数行列
   * @param inputNumber 入力番号
   * @param T 評価する時刻の列
   * @return {YY, XX} (出力と状態の応答) response
   */
  public static List<DoubleMatrix> ramp(DoubleRationalPolynomialMatrix G, int inputNumber, DoubleMatrix T) {
    if (inputNumber == 0) {
      List<DoubleMatrix> tmp = Tfm2ss.tfm2ss(G);
      DoubleMatrix A = tmp.get(0);
      DoubleMatrix B = tmp.get(1);
      DoubleMatrix C = tmp.get(2);
      DoubleMatrix D = tmp.get(3);
      return ramp(A, B, C, D, inputNumber, T);
    }
    List<DoubleMatrix> tmp = Tfm2ss.tfm2ss(G, inputNumber);
    DoubleMatrix A = tmp.get(0);
    DoubleMatrix B = tmp.get(1);
    DoubleMatrix C = tmp.get(2);
    DoubleMatrix D = tmp.get(3);
    return ramp(A, B, C, D, 1, T);
  }

  /**
   * 単位ランプ応答を計算します。
   * 
   * @param numerator 伝達関数の分子多項式の係数
   * @param denominator 伝達関数の分母多項式の係数
   * @param T 評価する時刻の列
   * @return {YY, XX} (出力と状態の応答) response
   */
  public static List<DoubleMatrix> ramp(DoubleMatrix numerator, DoubleMatrix denominator, DoubleMatrix T) {
    List<DoubleMatrix> tmp = Tf2ss.tf2ss(numerator, denominator);
    DoubleMatrix A = tmp.get(0);
    DoubleMatrix B = tmp.get(1);
    DoubleMatrix C = tmp.get(2);
    DoubleMatrix D = tmp.get(3);
    return ramp(A, B, C, D, 1, T);
  }

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

  /**
   * 単位ランプ応答をプロットします。
   * 
   * @param gnuplot gnuplot
   * @param g 伝達関数
   * @param T 評価する時刻の列
   * @return Gnuplot
   */
  public static Gnuplot plot(Gnuplot gnuplot, DoubleRationalPolynomial g, DoubleMatrix T) {
    List<DoubleMatrix> tmp = ramp(g, T);
    DoubleMatrix Y = tmp.get(0);

    Canvas canvas = gnuplot.createCanvas();
    canvas.setGridVisible(true);
    canvas.setTitle("Step response"); //$NON-NLS-1$
    canvas.setXLabel("t [sec]"); //$NON-NLS-1$
    canvas.setYLabel("y"); //$NON-NLS-1$
    canvas.plot(T, Y, new String[] {"y"}); //$NON-NLS-1$
    return gnuplot;
  }

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

  /**
   * 単位ランプ応答をプロットします。
   * 
   * @param gnuplot gnuplot
   * @param numerator 分子多項式の係数
   * @param denominator 分母多項式の係数
   * @param T 評価する時刻の列
   * @return Gnuplot
   */
  public static Gnuplot plot(Gnuplot gnuplot, DoubleMatrix numerator, DoubleMatrix denominator, DoubleMatrix T) {
    DoubleRationalPolynomial g = Tf2tfn.tf2tfn(numerator, denominator);
    return plot(gnuplot, g, T);
  }

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

  /**
   * 単位ランプ応答をプロットします。
   * 
   * @param gnuplot gnuplot
   * @param G 伝達関数ぎょうれう
   * @param inputNumber 入力番号
   * @param T 評価する時刻の列
   * @return Gnuplot
   */
  public static Gnuplot plot(Gnuplot gnuplot, DoubleRationalPolynomialMatrix G, int inputNumber, DoubleMatrix T) {
    List<DoubleMatrix> tmp = ramp(G, inputNumber, T);
    DoubleMatrix Y = tmp.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"; //$NON-NLS-1$
      } else {
        yy[i - 1] = "y" + i; //$NON-NLS-1$
      }
    }

    Canvas canvas = gnuplot.createCanvas();
    canvas.setGridVisible(true);
    canvas.setTitle("Step response"); //$NON-NLS-1$
    canvas.setXLabel("t [sec]"); //$NON-NLS-1$
    canvas.setYLabel("y"); //$NON-NLS-1$
    canvas.plot(T, Y, yy);
    return gnuplot;
  }

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

  /**
   * 単位ランプ応答をプロットします。
   * 
   * @param gnuplot gnuplot
   * @param A A行列
   * @param B B行列
   * @param C C行列
   * @param D D行列
   * @param inputNumber 入力番号
   * @param T 評価する時刻の列
   * @return Gnuplot
   */
  public static Gnuplot plot(Gnuplot gnuplot, DoubleMatrix A, DoubleMatrix B, DoubleMatrix C, DoubleMatrix D, int inputNumber, DoubleMatrix T) {
    List<DoubleMatrix> tmp = ramp(A, B, C, D, inputNumber, T);
    DoubleMatrix Y = tmp.get(0);
    DoubleMatrix X = tmp.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"; //$NON-NLS-1$
      } else {
        yy[i - 1] = "y" + i; //$NON-NLS-1$
      }
    }

    gnuplot.createCanvas(2, 1);
    Canvas canvas1 = gnuplot.getCanvas(0, 0);
    canvas1.setGridVisible(true);
    canvas1.setXLabel("t [sec]"); //$NON-NLS-1$
    canvas1.setYLabel("y"); //$NON-NLS-1$
    canvas1.setTitle("Step response"); //$NON-NLS-1$
    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; //$NON-NLS-1$
    }

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

}