Lsim.java

/*
 * $Id: Lsim.java,v 1.31 2008/03/23 23:41:55 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.scalar.DoubleNumber;
import org.mklab.tool.graph.gnuplot.Canvas;
import org.mklab.tool.graph.gnuplot.Gnuplot;


/**
 * 連続時間線形システムの任意入力に対する応答を求めるクラスです。
 * 
 * <p>Response of cont-time linear systems to given input
 * 
 * @author koga
 * @version $Revision: 1.31 $
 * @see org.mklab.tool.control.Dlsim
 */
public class Lsim {

  /**
   * 連続時間システム
   * 
   * <pre><code>
   * 
   * . x = Ax + Bu y = Cx + Du
   * 
   * </code></pre>
   * 
   * に入力の時系列 <code>U</code> が与えられた場合の時間応答を計算します。 行列 <code>U</code> の各列が各時刻の入力です。
   * 
   * @param A A行列
   * @param B B行列
   * @param C C行列
   * @param D D行列
   * @param U_ 入力の列
   * @param T 評価する時刻の列
   * @return {Y, X} (出力と状態の応答) response
   */
  public static List<DoubleMatrix> lsim(DoubleMatrix A, DoubleMatrix B, DoubleMatrix C, DoubleMatrix D, DoubleMatrix U_, DoubleMatrix T) {
    DoubleMatrix x0 = A.createZero(B.getRowSize(), 1);
    return lsim(A, B, C, D, U_, T, x0);
  }

  /**
   * 連続時間システム
   * 
   * <pre><code>
   * 
   * . Ex = Ax + Bu y = Cx + Du
   * 
   * </code></pre>
   * 
   * に入力の時系列 <code>U</code> が与えられた場合の時間応答を計算します。 行列 <code>U</code> の各列が各時刻の入力です。
   * 
   * @param A A行列
   * @param B B行列
   * @param C C行列
   * @param D D行列
   * @param E E行列
   * @param U_ 入力の列
   * @param T 評価する時刻の列
   * @return {Y, X} (出力と状態の応答) response
   */
  public static List<DoubleMatrix> lsimDes(DoubleMatrix A, DoubleMatrix B, DoubleMatrix C, DoubleMatrix D, DoubleMatrix E, DoubleMatrix U_, DoubleMatrix T) {
    DoubleMatrix x0 = A.createZero(B.getRowSize(), 1);
    return lsimDes(A, B, C, D, E, U_, T, x0);
  }

  /**
   * 連続時間応答を計算する
   * 
   * @param A_ A行列
   * @param B_ B行列
   * @param C C行列
   * @param D D行列
   * @param E_ E行列
   * @param U_ 入力の列
   * @param T_ 評価する時刻の列
   * @param x0_ 初期状態
   * @return {Y, X} (出力と状態の応答) response
   */
  public static List<DoubleMatrix> lsimDes(DoubleMatrix A_, DoubleMatrix B_, DoubleMatrix C, DoubleMatrix D, DoubleMatrix E_, DoubleMatrix U_, DoubleMatrix T_, DoubleMatrix x0_) {
    DoubleMatrix E = E_.createClone();
    DoubleMatrix A;
    DoubleMatrix B;
    if (E.isFullRank()) {
      DoubleMatrix invE = E.inverse();
      A = invE.multiply(A_);
      B = invE.multiply(B_);
    } else {
      throw new RuntimeException();
    }

    return lsim(A, B, C, D, U_, T_, x0_);
  }

  /**
   * 連続時間応答を計算する
   * 
   * @param A A行列
   * @param B B行列
   * @param C C行列
   * @param D D行列
   * @param U_ 入力の列
   * @param T 評価する時刻の列
   * @param x0_ 初期状態
   * @return {Y, X} (出力と状態の応答) response
   */
  public static List<DoubleMatrix> lsim(DoubleMatrix A, DoubleMatrix B, DoubleMatrix C, DoubleMatrix D, DoubleMatrix U_, DoubleMatrix T, DoubleMatrix x0_) {
    String message;
    if ((message = Abcdchk.abcdchk(A, B, C, D)).length() > 0) {
      throw new RuntimeException(message);
    }

    int stateSize = B.getRowSize();
    int inputSize = B.getColumnSize();
    int N = U_.getColumnSize();

    DoubleMatrix U = U_.createClone();
    DoubleMatrix x0 = x0_.createClone();

    DoubleNumber dt = T.getElement(2).subtract(T.getElement(1));

    List<DoubleMatrix> tmp = Series.series(A.createZero(inputSize), A.createUnit(inputSize), A.createUnit(inputSize), A.createZero(inputSize), A, B, C, D);
    // augmented system
    DoubleMatrix Aa = tmp.get(0);
    DoubleMatrix Ba = tmp.get(1);
    DoubleMatrix Ca = tmp.get(2);
    DoubleMatrix Da = tmp.get(3);

    List<DoubleMatrix> tmp3 = C2d.c2d(Aa, Ba, dt);
    DoubleMatrix Aad = tmp3.get(0);
    DoubleMatrix Bad = tmp3.get(1);

    // Initial states for the augmented system
    DoubleMatrix tmp1 = A.createZero(inputSize, 1).appendDown(x0);
    DoubleMatrix tmp2 = Ba.multiply(U.getColumnVector(1));
    x0 = tmp1.add(tmp2);

    U.setColumnVectors(1, N - 1, U.getColumnVectors(2, N).subtract(U.getColumnVectors(1, N - 1)).divide(dt));
    U.setColumnVector(N, U.getColumnVector(N - 1));

    DoubleMatrix Xa = Ltitr.ltitr(Aad, Bad, U, x0);
    DoubleMatrix Y = Ca.multiply(Xa).add(Da.multiply(U));

    // State of the original system
    DoubleMatrix X = Xa.getSubMatrix(inputSize + 1, inputSize + stateSize, 1, Xa.getColumnSize());

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

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

  /**
   * 連続時間応答をプロットする
   * 
   * @param gnuplot gnuplot
   * @param A A行列
   * @param B B行列
   * @param C C行列
   * @param D D行列
   * @param U 入力の列
   * @param T 評価する時刻の列
   * @return Gnuplot
   */
  public static Gnuplot plot(Gnuplot gnuplot, DoubleMatrix A, DoubleMatrix B, DoubleMatrix C, DoubleMatrix D, DoubleMatrix U, DoubleMatrix T) {
    DoubleMatrix x0 = new DoubleMatrix(B.getRowSize(), 1);
    return plot(gnuplot, A, B, C, D, U, T, x0);
  }

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

  /**
   * 連続時間応答をプロットする
   * 
   * @param gnuplot gnuplot
   * @param A A行列
   * @param B B行列
   * @param C C行列
   * @param D D行列
   * @param U 入力の列
   * @param T 評価する時刻の列
   * @param x0 初期状態
   * @return Gnuplot
   */
  @SuppressWarnings("nls")
  public static Gnuplot plot(Gnuplot gnuplot, DoubleMatrix A, DoubleMatrix B, DoubleMatrix C, DoubleMatrix D, DoubleMatrix U, DoubleMatrix T, DoubleMatrix x0) {

    String message = Abcdchk.abcdchk(A, B, C, D);

    if (message.length() > 0) {
      throw new RuntimeException(message);
    }

    List<DoubleMatrix> tmp = lsim(A, B, C, D, U, T, x0);
    DoubleMatrix Y = tmp.get(0);
    DoubleMatrix X = tmp.get(1);

    int stateSize = A.getRowSize();
    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);

    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;
  }

  /**
   * 連続時間応答をプロットする
   * 
   * @param gnuplot gnuplot
   * @param A_ A行列
   * @param B_ B行列
   * @param C_ C行列
   * @param D_ D行列
   * @param E_ E行列
   * @param U 入力の列
   * @param T 評価する時刻の列
   * @param x0 初期状態
   * @return Gnuplot
   */
  public static Gnuplot plotDes(Gnuplot gnuplot, DoubleMatrix A_, DoubleMatrix B_, DoubleMatrix C_, DoubleMatrix D_, DoubleMatrix E_, DoubleMatrix U, DoubleMatrix T, DoubleMatrix x0) {
    List<DoubleMatrix> tmp = removeE(A_, B_, C_, D_, E_);
    DoubleMatrix A = tmp.get(0);
    DoubleMatrix B = tmp.get(1);
    DoubleMatrix C = tmp.get(2);
    DoubleMatrix D = tmp.get(3);

    return plot(gnuplot, A, B, C, D, U, T, x0);
  }

  /**
   * 連続時間応答をプロットする
   * 
   * @param A_ A行列
   * @param B_ B行列
   * @param C_ C行列
   * @param D_ D行列
   * @param E_ E行列
   * @param U 入力の列
   * @param T 評価する時刻の列
   * @param x0 初期状態
   * @return Gnuplot
   * @throws IOException gnuplotプロセスを起動できない場合
   */
  public static Gnuplot plotDes(DoubleMatrix A_, DoubleMatrix B_, DoubleMatrix C_, DoubleMatrix D_, DoubleMatrix E_, DoubleMatrix U, DoubleMatrix T, DoubleMatrix x0) throws IOException {
    List<DoubleMatrix> tmp = removeE(A_, B_, C_, D_, E_);
    DoubleMatrix A = tmp.get(0);
    DoubleMatrix B = tmp.get(1);
    DoubleMatrix C = tmp.get(2);
    DoubleMatrix D = tmp.get(3);
    return plot(A, B, C, D, U, T, x0);
  }

  /**
   * 連続時間応答をプロットする
   * 
   * @param A_ A行列
   * @param B_ B行列
   * @param C_ C行列
   * @param D_ D行列
   * @param E_ E行列
   * @param U 入力の列
   * @param T 評価する時刻の列
   * @return Gnuplot
   * @throws IOException gnuplotプロセスを起動できない場合
   */
  public static Gnuplot plotDes(DoubleMatrix A_, DoubleMatrix B_, DoubleMatrix C_, DoubleMatrix D_, DoubleMatrix E_, DoubleMatrix U, DoubleMatrix T) throws IOException {
    List<DoubleMatrix> tmp = removeE(A_, B_, C_, D_, E_);
    DoubleMatrix A = tmp.get(0);
    DoubleMatrix B = tmp.get(1);
    DoubleMatrix C = tmp.get(2);
    DoubleMatrix D = tmp.get(3);
    return plot(A, B, C, D, U, T);
  }

  /**
   * 連続時間応答をプロットする
   * 
   * @param gnuplot gnuplot
   * @param A_ A行列
   * @param B_ B行列
   * @param C_ C行列
   * @param D_ D行列
   * @param E_ E行列
   * @param U 入力の列
   * @param T 評価する時刻の列
   * @return Gnuplot
   */
  public static Gnuplot plotDes(Gnuplot gnuplot, DoubleMatrix A_, DoubleMatrix B_, DoubleMatrix C_, DoubleMatrix D_, DoubleMatrix E_, DoubleMatrix U, DoubleMatrix T) {
    List<DoubleMatrix> tmp = removeE(A_, B_, C_, D_, E_);
    DoubleMatrix A = tmp.get(0);
    DoubleMatrix B = tmp.get(1);
    DoubleMatrix C = tmp.get(2);
    DoubleMatrix D = tmp.get(3);

    return plot(gnuplot, A, B, C, D, U, T);
  }

  private static List<DoubleMatrix> removeE(DoubleMatrix A_, DoubleMatrix B_, DoubleMatrix C_, DoubleMatrix D_, DoubleMatrix E_) {
    final DoubleMatrix E = E_.createClone();
    if (E.isFullRank()) {
      final DoubleMatrix invE = E.inverse();
      final DoubleMatrix A = invE.multiply(A_);
      final DoubleMatrix B = invE.multiply(B_);
      final DoubleMatrix C = C_.createClone();
      final DoubleMatrix D = D_.createClone();
      return new ArrayList<>(Arrays.asList(new DoubleMatrix[] {A, B, C, D}));
    }
    return Tfm2ss.tfm2ss(Des2tfm.des2tfm(A_, B_, C_, D_, E_));
  }
}