Tfm2ss.java
/*
* $Id: Tfm2ss.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.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import org.mklab.nfc.matrix.AnyRealRationalPolynomialMatrix;
import org.mklab.nfc.matrix.ComplexNumericalMatrix;
import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.matrix.DoubleRationalPolynomialMatrix;
import org.mklab.nfc.matrix.RealNumericalMatrix;
import org.mklab.nfc.scalar.AnyRealRationalPolynomial;
import org.mklab.nfc.scalar.ComplexNumericalScalar;
import org.mklab.nfc.scalar.DoubleRationalPolynomial;
import org.mklab.nfc.scalar.RealNumericalScalar;
import org.mklab.tool.matrix.Diag;
/**
* 伝達関数行列(有理多項式行列)から状態空間表現に変換するクラスです。
*
* <p>Transfer function matrix to state-space conversion
*
* @author koga
* @version $Revision: 1.28 $
* @see org.mklab.tool.control.Tfm2tf
* @see org.mklab.tool.control.Tfm2tfn
* @see org.mklab.tool.control.Tfm2zp
* @see org.mklab.tool.control.Ss2tfm
*/
public class Tfm2ss {
/**
* 伝達関数行列が
*
* <pre><code> -1 G(s) = C (sI - A) B + D </code></pre>
*
* であるシステムの状態空間表現
*
* <pre><code> . x = Ax + Bu y = Cx + Du </code></pre>
*
* を求めます。
*
* @param G 伝達関数行列
* @return {A,B,C,D} (状態空間表現) (state-space representation)
*/
public static List<DoubleMatrix> tfm2ss(DoubleRationalPolynomialMatrix G) {
double tolerance = -1.0;
int inputNumber = 0;
return tfm2ss(G, inputNumber, tolerance);
}
/**
* <code>iu</code>番目の入力から出力までのシステムの 状態空間表現を求めます。
*
* @param G 伝達関数行列
* @param inputNumber 入力番号
* @return {A,B,C,D} (状態空間表現) state-space representation
*/
public static List<DoubleMatrix> tfm2ss(DoubleRationalPolynomialMatrix G, int inputNumber) {
double tolerance = -1.0;
return tfm2ss(G, inputNumber, tolerance);
}
/**
* 最小実現をminreal()で求める際に、許容誤差<code>tol</code> を用いる。
*
* @param G 伝達関数行列
* @param inputNumber 入力番号
* @param tolerance 最小実現で用いる許容誤差
* @return {A,B,C,D} (状態空間表現) (state-space representation)
*/
public static List<DoubleMatrix> tfm2ss(DoubleRationalPolynomialMatrix G, int inputNumber, double tolerance) {
int inputSize = G.getColumnSize();
int outputSize = G.getRowSize();
int is, it;
if (inputNumber == 0) {
is = 1;
it = inputSize;
} else {
is = inputNumber;
it = inputNumber;
}
DoubleMatrix A = null, B = null, C = null, D = null;
DoubleMatrix AA = null, BB = null, CC = null, DD = null;
for (int i = is; i <= it; i++) {
for (int j = 1; j <= outputSize; j++) {
final DoubleRationalPolynomial g = G.getElement(j, i);
DoubleMatrix num = g.getNumerator().getCoefficients().flipLeftRight();
DoubleMatrix den = g.getDenominator().getCoefficients().flipLeftRight();
if (num.length() < den.length()) {
num = num.createZero(1, den.length() - num.length()).appendRight(num);
}
final List<DoubleMatrix> tmp1 = Tf2ss.tf2ss(num, den);
DoubleMatrix a = tmp1.get(0);
DoubleMatrix b = tmp1.get(1);
DoubleMatrix c = tmp1.get(2);
DoubleMatrix d = tmp1.get(3);
final List<DoubleMatrix> tmp2 = Minreal.minreal(a, b, c, d, tolerance);
a = tmp2.get(0);
b = tmp2.get(1);
c = tmp2.get(2);
d = tmp2.get(3);
// a, b, c, d matrices are returned in controllable canonical form
// Revised with the advice of Dr. Nonaka (1999.11.04)
if (a.getRowSize() == 0) {
// G(j,i) is constant
a = num.createZero(0, 0);
b = num.createZero(0, 1);
c = num.createZero(1, 0);
}
if (j == 1) {
A = a;
B = b;
C = c;
D = d;
} else {
A = Diag.diag(A, a);
DoubleMatrix b2 = B;
B = b2.appendDown(b);
DoubleMatrix c2 = C;
if (c.getColumnSize() == 0) {
C = c2.appendDown(c.createZero(1, c2.getColumnSize()));
} else {
C = Diag.diag(c2, c);
}
DoubleMatrix d2 = D;
D = d2.appendDown(d);
final List<DoubleMatrix> tmp3 = Minreal.minreal(A, B, C, D, tolerance);
A = tmp3.get(0);
B = tmp3.get(1);
C = tmp3.get(2);
D = tmp3.get(3);
}
}
if (i == is) {
AA = A;
BB = B;
CC = C;
DD = D;
} else {
AA = Diag.diag(AA, A);
BB = Diag.diag(BB, B);
DoubleMatrix cC2 = CC;
CC = cC2.appendRight(C);
DoubleMatrix dD2 = DD;
DD = dD2.appendRight(D);
List<DoubleMatrix> tmp4 = Minreal.minreal(AA, BB, CC, DD, tolerance);
AA = tmp4.get(0);
BB = tmp4.get(1);
CC = tmp4.get(2);
DD = tmp4.get(3);
}
}
return new ArrayList<>(Arrays.asList(new DoubleMatrix[] {AA, BB, CC, DD}));
}
/**
* 最小実現をminreal()で求める際に、許容誤差<code>tol</code> を用いる。
*
* @param G 伝達関数行列
* @param inputNumber 入力番号
* @param tolerance 最小実現で用いる許容誤差
* @return {A,B,C,D} (状態空間表現) (state-space representation)
* @param <RS> type of real scalar
* @param <RM> type of real matrix
* @param <CS> type of complex scalar
* @param <CM> type of complex matrix
*/
public static <RS extends RealNumericalScalar<RS, RM, CS, CM>, RM extends RealNumericalMatrix<RS, RM, CS, CM>, CS extends ComplexNumericalScalar<RS, RM, CS, CM>, CM extends ComplexNumericalMatrix<RS, RM, CS, CM>> List<RM> tfm2ss(
AnyRealRationalPolynomialMatrix<RS, RM, CS, CM> G, int inputNumber, RS tolerance) {
int inputSize = G.getColumnSize();
int outputSize = G.getRowSize();
int is, it;
if (inputNumber == 0) {
is = 1;
it = inputSize;
} else {
is = inputNumber;
it = inputNumber;
}
RM A = null, B = null, C = null, D = null;
RM AA = null, BB = null, CC = null, DD = null;
for (int i = is; i <= it; i++) {
for (int j = 1; j <= outputSize; j++) {
final AnyRealRationalPolynomial<RS, RM, CS, CM> g = G.getElement(j, i);
RM num = g.getNumerator().getCoefficients().flipLeftRight();
RM den = g.getDenominator().getCoefficients().flipLeftRight();
if (num.length() < den.length()) {
num = num.createZero(1, den.length() - num.length()).appendRight(num);
}
final List<RM> tmp1 = Tf2ss.tf2ss(num, den);
RM a = tmp1.get(0);
RM b = tmp1.get(1);
RM c = tmp1.get(2);
RM d = tmp1.get(3);
final List<RM> tmp2 = Minreal.minreal(a, b, c, d, tolerance);
a = tmp2.get(0);
b = tmp2.get(1);
c = tmp2.get(2);
d = tmp2.get(3);
// a, b, c, d matrices are returned in controllable canonical form
// Revised with the advice of Dr. Nonaka (1999.11.04)
if (a.getRowSize() == 0) {
// G(j,i) is constant
a = num.createZero(0, 0);
b = num.createZero(0, 1);
c = num.createZero(1, 0);
}
if (j == 1) {
A = a;
B = b;
C = c;
D = d;
} else {
A = Diag.diag(A, a);
RM b2 = B;
B = b2.appendDown(b);
RM c2 = C;
if (c.getColumnSize() == 0) {
C = c2.appendDown(c.createZero(1, c2.getColumnSize()));
} else {
C = Diag.diag(c2, c);
}
RM d2 = D;
D = d2.appendDown(d);
final List<RM> tmp3 = Minreal.minreal(A, B, C, D, tolerance);
A = tmp3.get(0);
B = tmp3.get(1);
C = tmp3.get(2);
D = tmp3.get(3);
}
}
if (i == is) {
AA = A;
BB = B;
CC = C;
DD = D;
} else {
AA = Diag.diag(AA, A);
BB = Diag.diag(BB, B);
RM cC2 = CC;
CC = cC2.appendRight(C);
RM dD2 = DD;
DD = dD2.appendRight(D);
List<RM> tmp4 = Minreal.minreal(AA, BB, CC, DD, tolerance);
AA = tmp4.get(0);
BB = tmp4.get(1);
CC = tmp4.get(2);
DD = tmp4.get(3);
}
}
List<RM> abcd = new ArrayList<>();
abcd.add(AA);
abcd.add(BB);
abcd.add(CC);
abcd.add(DD);
return abcd;
}
/**
* 伝達関数行列が
*
* <pre><code> -1 G(s) = C (sI - A) B + D </code></pre>
*
* であるシステムの状態空間表現
*
* <pre><code> . x = Ax + Bu y = Cx + Du </code></pre>
*
* を求めます。
*
* @param G 伝達関数行列
* @return {A,B,C,D} (状態空間表現) (state-space representation)
* @param <RS> type of real scalar
* @param <RM> type of real matrix
* @param <CS> type of complex scalar
* @param <CM> type of complex matrix
*/
public static <RS extends RealNumericalScalar<RS, RM, CS, CM>, RM extends RealNumericalMatrix<RS, RM, CS, CM>, CS extends ComplexNumericalScalar<RS, RM, CS, CM>, CM extends ComplexNumericalMatrix<RS, RM, CS, CM>> List<RM> tfm2ss(
AnyRealRationalPolynomialMatrix<RS, RM, CS, CM> G) {
RS sunit = G.getElement(1, 1).getNumerator().getCoefficient(0).createUnit();
RS tolerance = sunit.create(-1);
int inputNumber = 0;
return tfm2ss(G, inputNumber, tolerance);
}
}