Step.java
/*
* $Id: Step.java,v 1.37 2008/03/25 01:12:40 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.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>Step response of continuous-time linear systems
*
* @author koga
* @version $Revision: 1.37 $
* @see org.mklab.tool.control.Impulse
* @see org.mklab.tool.control.Dstep
*/
public class Step {
/**
* メインメソッド
*
* @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>iu</code> 番目の入力に単位ステップ入力が加えられた 場合のステップ応答を求めます。
*
* <p>もし <code>iu = 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]] と [X 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> step(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;
// integrator
DoubleMatrix aI = A.createZero(1, 1);
DoubleMatrix bI = A.createUnit(1);
DoubleMatrix cI = A.createUnit(1);
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> abcd = Series.series(aI, bI, cI, dI, A, B.getColumnVector(i), C, D.getColumnVector(i));
DoubleMatrix A2 = abcd.get(0);
DoubleMatrix B2 = abcd.get(1);
DoubleMatrix C2 = abcd.get(2);
DoubleMatrix D2 = abcd.get(3);
List<DoubleMatrix> yx = Impulse.impulse(A2, B2, C2, D2, 1, T);
DoubleMatrix Y = yx.get(0);
DoubleMatrix X = yx.get(1);
X = X.getRowVectors(2, X.getRowSize()); // remove the state of integrator
YY.setSubMatrix(i, 1, Y, Y);
XX.setSubMatrix(i, 1, X, X);
}
} else {
List<DoubleMatrix> abcd = Series.series(aI, bI, cI, dI, A, B.getColumnVector(inputNumber), C, D.getColumnVector(inputNumber));
DoubleMatrix A2 = abcd.get(0);
DoubleMatrix B2 = abcd.get(1);
DoubleMatrix C2 = abcd.get(2);
DoubleMatrix D2 = abcd.get(3);
List<DoubleMatrix> yx = Impulse.impulse(A2, B2, C2, D2, 1, T);
YY = yx.get(0);
XX = yx.get(1);
XX = XX.getRowVectors(2, XX.getRowSize()); // remove the state of integrator
}
return new ArrayList<>(Arrays.asList(new DoubleMatrix[] {YY, XX}));
}
/**
* 単位ステップ応答を計算します。
*
* @param g 伝達関数
* @param T 時刻の列
* @return {YY, XX} (出力と状態の応答) response
*/
public static List<DoubleMatrix> step(DoubleRationalPolynomial g, 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 step(A, B, C, D, 1, T);
}
/**
* 単位ステップ応答を計算します。
*
* @param system 線形システム
* @param T 時刻の列
* @return {YY, XX} (出力と状態の応答) response
*/
public static List<DoubleMatrix> step(DoubleLinearSystem system, DoubleMatrix T) {
if (system.isProper()) {
DoubleMatrix A = system.getA();
DoubleMatrix B = system.getB();
DoubleMatrix C = system.getC();
DoubleMatrix D = system.getD();
return step(A, B, C, D, 1, T);
}
return step(system.getTransferFunctionMatrix(), 1, T);
}
/**
* 単位ステップ応答を計算します。
*
* @param G 伝達関数行列
* @param inputNumber 入力番号
* @param T 評価する時刻の列
* @return {YY, XX} (出力と状態の応答) response
*/
public static List<DoubleMatrix> step(DoubleRationalPolynomialMatrix G, int inputNumber, DoubleMatrix T) {
if (inputNumber == 0) {
List<DoubleMatrix> abcd = Tfm2ss.tfm2ss(G);
DoubleMatrix A = abcd.get(0);
DoubleMatrix B = abcd.get(1);
DoubleMatrix C = abcd.get(2);
DoubleMatrix D = abcd.get(3);
return step(A, B, C, D, inputNumber, T);
}
List<DoubleMatrix> abcd = Tfm2ss.tfm2ss(G, inputNumber);
DoubleMatrix A = abcd.get(0);
DoubleMatrix B = abcd.get(1);
DoubleMatrix C = abcd.get(2);
DoubleMatrix D = abcd.get(3);
return step(A, B, C, D, 1, T);
}
/**
* 単位ステップ応答を計算します。
*
* @param numerator 伝達関数の分子多項式の係数
* @param denominator 伝達関数の分母多項式の係数
* @param T 評価する時刻の列
* @return {YY, XX} (出力と状態の応答) response
*/
public static List<DoubleMatrix> step(DoubleMatrix numerator, DoubleMatrix denominator, 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 step(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
*/
@SuppressWarnings("nls")
public static Gnuplot plot(Gnuplot gnuplot, DoubleRationalPolynomial g, DoubleMatrix T) {
List<DoubleMatrix> yx = step(g, T);
DoubleMatrix Y = yx.get(0);
Canvas canvas = gnuplot.createCanvas();
canvas.setGridVisible(true);
canvas.setTitle("Step response");
canvas.setXLabel("t [sec]");
canvas.setYLabel("y");
canvas.plot(T, Y, new String[] {"y"});
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
*/
@SuppressWarnings("nls")
public static Gnuplot plot(Gnuplot gnuplot, DoubleRationalPolynomialMatrix G, int inputNumber, DoubleMatrix T) {
List<DoubleMatrix> yx = step(G, inputNumber, 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.setTitle("Step response");
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 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
*/
@SuppressWarnings("nls")
public static Gnuplot plot(Gnuplot gnuplot, DoubleMatrix A, DoubleMatrix B, DoubleMatrix C, DoubleMatrix D, int inputNumber, DoubleMatrix T) {
List<DoubleMatrix> yx = step(A, B, C, D, inputNumber, 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.setTitle("Step response");
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.setTitle("");
canvas2.setXLabel("t [sec]");
canvas2.setYLabel("x");
canvas2.plot(T, X, xx);
return gnuplot;
}
}