Tf2ss.java
/*
* $Id: Tf2ss.java,v 1.26 2008/07/17 07:30:03 koga Exp $
*
* Copyright (C) 2004 Koga Laboratory. All rights reserved.
*/
package org.mklab.tool.control;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import org.mklab.nfc.matrix.BooleanMatrix;
import org.mklab.nfc.matrix.ComplexNumericalMatrix;
import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.matrix.IntMatrix;
import org.mklab.nfc.matrix.RealNumericalMatrix;
import org.mklab.nfc.scalar.ComplexNumericalScalar;
import org.mklab.nfc.scalar.DoublePolynomial;
import org.mklab.nfc.scalar.RealNumericalScalar;
/**
* 伝達関数(係数からなる行列)から状態空間表現に変換するクラスです。
*
* <p>
* Transfer function to state-space conversion
*
* @author koga
* @version $Revision: 1.26 $
* @see org.mklab.tool.control.Tf2zp
* @see org.mklab.tool.control.Tf2tfn
* @see org.mklab.tool.control.Tf2tfm
* @see org.mklab.tool.control.Ss2tf
*/
public class Tf2ss {
/**
* メインメソッド
*
* @param args コマンドライン引数
*/
public static void main(String[] args) {
// s^2 + 2*s + 3
// -------------------
// s^3 + 2*s^2 + 3*s + 4
DoubleMatrix NUM = new DoubleMatrix(new double[] { 3, 2, 1 });
DoubleMatrix den = new DoubleMatrix(new double[] { 4, 3, 2, 1 });
List<DoubleMatrix> ss = tf2ss(NUM, den);
DoubleMatrix A = ss.get(0);
DoubleMatrix B = ss.get(1);
DoubleMatrix C = ss.get(2);
DoubleMatrix D = ss.get(3);
A.print();
B.print();
C.print();
D.print();
List<DoubleMatrix> tf = Ss2tf.ss2tf(A, B, C, D);
new DoublePolynomial(tf.get(0)).print("n"); //$NON-NLS-1$
new DoublePolynomial(tf.get(1)).print("d"); //$NON-NLS-1$
}
/**
* 1入力多出力システム
*
* <pre>
* <code>
*
* NUM(s) G(s) = -------- den(s)
*
* </code>
* </pre>
*
* の状態空間表現
*
* <pre>
* <code>
*
* . x = Ax + Bu y = Cx + Du
*
* </code>
* </pre>
*
* を求めます。 係数行列 <code>A,B,C,D</code> は、可制御正準形で返されます。
*
* @param numeratorMatrix 伝達関数の分子多項式の係数
* @param denominatorMatrix 伝達関数の分母多項式の係数
* @return {A,B,C,D} 状態空間表現 (state space representation)
*/
public static List<DoubleMatrix> tf2ss(DoubleMatrix numeratorMatrix, DoubleMatrix denominatorMatrix) {
DoubleMatrix NUM = numeratorMatrix;
DoubleMatrix den = denominatorMatrix;
// Strip leading zeros
IntMatrix idx = den.compareElementWise(".!=", 0).find(); //$NON-NLS-1$
if (idx.length() > 0) {
den = den.getSubVector(idx.getIntElement(1), den.length());
} else {
den = den.createOnes(1); // new DoubleMatrix(new double[] {1});
}
int stateSize = den.length();
int nn = NUM.getColumnSize();
if (nn > stateSize) {
// Strip leading zeros;
BooleanMatrix tmp2 = NUM.getColumnVectors(1, nn - stateSize).compareElementWise(".==", 0); //$NON-NLS-1$
if (tmp2.allTrue()) {
NUM = NUM.getColumnVectors(nn - stateSize + 1, nn);
nn = NUM.getColumnSize();
} else {
throw new RuntimeException(Messages.getString("Tf2ss.0")); //$NON-NLS-1$
}
}
// Pad numerator with leading zeros and normalize
NUM = den.createZero(NUM.getRowSize(), stateSize - nn).appendRight(NUM).divide(den.getElement(1));
DoubleMatrix D = NUM.getColumnVector(1);
if (stateSize == 1) {
DoubleMatrix A = den.createZero(0, 0);
DoubleMatrix B = den.createZero(0, 1);
DoubleMatrix C = den.createZero(1, 0);
return new ArrayList<>(Arrays.asList(new DoubleMatrix[] { A, B, C, D }));
}
den = den.getSubVector(2, stateSize).divide(den.getElement(1));
DoubleMatrix A = den.unaryMinus().appendDown(den.createUnit(stateSize - 2, stateSize - 1));
DoubleMatrix B = den.createUnit(stateSize - 1, 1);
DoubleMatrix C = NUM.getColumnVectors(2, stateSize).subtract(NUM.getColumnVector(1).multiply(den));
return new ArrayList<>(Arrays.asList(new DoubleMatrix[] { A, B, C, D }));
}
/**
* 1入力多出力システム
*
* <pre>
* <code>
*
* NUM(s) G(s) = -------- den(s)
*
* </code>
* </pre>
*
* の状態空間表現
*
* <pre>
* <code>
*
* . x = Ax + Bu y = Cx + Du
*
* </code>
* </pre>
*
* を求めます。 係数行列 <code>A,B,C,D</code> は、可制御正準形で返されます。
*
* @param <RES> 実係数スカラーの型
* @param <REM> 実係数行列の型
* @param <CES> 複素係数スカラーの型
* @param <CEM> 複素係数行列の型
* @param numeratorMatrix 伝達関数の分子多項式の係数
* @param denominatorMatrix 伝達関数の分母多項式の係数
* @return {A,B,C,D} 状態空間表現 (state space representation)
*/
public static <RES extends RealNumericalScalar<RES, REM, CES, CEM>, REM extends RealNumericalMatrix<RES, REM, CES, CEM>, CES extends ComplexNumericalScalar<RES, REM, CES, CEM>, CEM extends ComplexNumericalMatrix<RES, REM, CES, CEM>> List<REM> tf2ss(
REM numeratorMatrix, REM denominatorMatrix) {
REM NUM = numeratorMatrix;
REM den = denominatorMatrix;
// Strip leading zeros
IntMatrix idx = den.compareElementWise(".!=", 0).find(); //$NON-NLS-1$
if (idx.length() > 0) {
den = den.getSubVector(idx.getIntElement(1), den.length());
} else {
den = den.createOnes(1); // new DoubleMatrix(new double[] {1});
}
int stateSize = den.length();
int nn = NUM.getColumnSize();
if (nn > stateSize) {
// Strip leading zeros;
BooleanMatrix tmp2 = NUM.getColumnVectors(1, nn - stateSize).compareElementWise(".==", 0); //$NON-NLS-1$
if (tmp2.allTrue()) {
NUM = NUM.getColumnVectors(nn - stateSize + 1, nn);
nn = NUM.getColumnSize();
} else {
throw new RuntimeException(Messages.getString("Tf2ss.0")); //$NON-NLS-1$
}
}
// Pad numerator with leading zeros and normalize
NUM = den.createZero(NUM.getRowSize(), stateSize - nn).appendRight(NUM).divide(den.getElement(1));
REM D = NUM.getColumnVector(1);
if (stateSize == 1) {
REM A = den.createZero(0, 0);
REM B = den.createZero(0, 1);
REM C = den.createZero(1, 0);
List<REM> ans = new ArrayList<>();
ans.add(A);
ans.add(B);
ans.add(C);
ans.add(D);
return ans;
}
den = den.getSubVector(2, stateSize).divide(den.getElement(1));
REM A = den.unaryMinus().appendDown(den.createUnit(stateSize - 2, stateSize - 1));
REM B = den.createUnit(stateSize - 1, 1);
REM C = NUM.getColumnVectors(2, stateSize).subtract(NUM.getColumnVector(1).multiply(den));
List<REM> ans = new ArrayList<>();
ans.add(A);
ans.add(B);
ans.add(C);
ans.add(D);
return ans;
}
}