Des2tfm.java
/*
* Created on 2009/01/28
* Copyright (C) 2009 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.AnyRealPolynomialMatrix;
import org.mklab.nfc.matrix.AnyRealRationalPolynomialMatrix;
import org.mklab.nfc.matrix.ComplexNumericalMatrix;
import org.mklab.nfc.matrix.DoubleComplexMatrix;
import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.matrix.DoublePolynomialMatrix;
import org.mklab.nfc.matrix.DoubleRationalPolynomialMatrix;
import org.mklab.nfc.matrix.RealNumericalMatrix;
import org.mklab.nfc.scalar.AnyRealPolynomial;
import org.mklab.nfc.scalar.AnyRealRationalPolynomial;
import org.mklab.nfc.scalar.ComplexNumericalScalar;
import org.mklab.nfc.scalar.DoubleComplexNumber;
import org.mklab.nfc.scalar.DoubleNumber;
import org.mklab.nfc.scalar.DoublePolynomial;
import org.mklab.nfc.scalar.DoubleRationalPolynomial;
import org.mklab.nfc.scalar.RealNumericalScalar;
/**
* ディスクリプタ表現から伝達関数行列(有理多項式行列)に変換するクラスです。 <p>copy of Ss2tf.java</p>
*
* @author Anan
*
*/
public class Des2tfm {
/**
* @param A ゲイン行列
* @param B 入力行列
* @param C 出力行列
* @param D ゲイン行列
* @param E ディスクリプター行列
* @return TransferFunctionMatrix
*/
public static DoubleRationalPolynomialMatrix des2tfm(DoubleMatrix A, DoubleMatrix B, DoubleMatrix C, DoubleMatrix D, DoubleMatrix E) {
DoubleMatrix numericalA = A;
DoubleNumber eps = numericalA.getElement(1, 1).getMachineEpsilon();
double defaultTolerance = numericalA.frobNorm().multiply(eps).doubleValue();
boolean defaultSimplify = true;
return des2tfm(A, B, C, D, E, defaultTolerance, defaultSimplify);
}
/**
* @param A ゲイン行列
* @param B 入力行列
* @param C 出力行列
* @param D ゲイン行列
* @param E ディスクリプター行列
* @param simplify 簡単化するならばtrue
* @return TransferFunctionMatrix
*/
public static DoubleRationalPolynomialMatrix des2tfm(DoubleMatrix A, DoubleMatrix B, DoubleMatrix C, DoubleMatrix D, DoubleMatrix E, boolean simplify) {
DoubleMatrix numericalA = A;
DoubleNumber eps = numericalA.getElement(1, 1).getMachineEpsilon();
double defaultTolerance = numericalA.frobNorm().multiply(eps).doubleValue() * A.getRowSize();
return des2tfm(A, B, C, D, E, defaultTolerance, simplify);
}
/**
* @param A ゲイン行列
* @param B 入力行列
* @param C 出力行列
* @param D ゲイン行列
* @param E ディスクリプター行列
* @param tolerance 許容誤差
* @return TransferFunctionMatrix
*/
public static DoubleRationalPolynomialMatrix des2tfm(DoubleMatrix A, DoubleMatrix B, DoubleMatrix C, DoubleMatrix D, DoubleMatrix E, double tolerance) {
boolean defaultSimplify = true;
return des2tfm(A, B, C, D, E, tolerance, defaultSimplify);
}
/**
* @param A ゲイン行列
* @param B 入力行列
* @param C 出力行列
* @param D ゲイン行列
* @param E ディスクリプター行列
* @param tolerance 許容誤差
* @param simplify 簡単化するならばtrue
* @return TransferFunctionMatrix
*/
public static DoubleRationalPolynomialMatrix des2tfm(DoubleMatrix A, DoubleMatrix B, DoubleMatrix C, DoubleMatrix D, DoubleMatrix E, double tolerance, boolean simplify) {
DoubleRationalPolynomialMatrix simpG = new DoubleRationalPolynomialMatrix();
String message;
if ((message = Abcdchk.abcdchk(A, B, C, D)).length() > 0) {
throw new RuntimeException(message);
}
List<DoubleMatrix> para = new WeierstrassForm<DoubleNumber, DoubleMatrix, DoubleComplexNumber, DoubleComplexMatrix>().weiestrassformByHessenberg(Arrays.asList(new DoubleMatrix[] {A, B, C, D, E}),
new DoubleNumber(tolerance));
DoubleMatrix a = para.get(0);
DoubleMatrix b = para.get(1);
DoubleMatrix c = para.get(2);
//DoubleMatrix d = para.get(3);
DoubleMatrix e = para.get(4);
int zeroCount = 0;
for (int i = 1; i <= E.getColumnSize(); i++) {
if (e.getElement(i, i).isZero(tolerance)) {
zeroCount = i - 1;
break;
}
}
DoubleMatrix As = a.getSubMatrix(1, zeroCount, 1, zeroCount);
DoubleMatrix Af = e.getSubMatrix(1 + zeroCount, e.getColumnSize(), 1 + zeroCount, e.getColumnSize());
DoubleMatrix Bs = b.getSubMatrix(1, zeroCount, 1, b.getColumnSize());
DoubleMatrix Bf = b.getSubMatrix(1 + zeroCount, b.getRowSize(), 1, b.getColumnSize());
DoubleMatrix Cs = c.getSubMatrix(1, c.getRowSize(), 1, zeroCount);
DoubleMatrix Cf = c.getSubMatrix(1, c.getRowSize(), zeroCount + 1, c.getColumnSize());
DoubleRationalPolynomialMatrix Gs = Ss2tfm.ss2tfm(As, Bs, Cs, D);
DoubleMatrix[] invIsAfCofficient = new DoubleMatrix[Af.getColumnSize()];
invIsAfCofficient[0] = Af.createUnit();
for (int i = 1; i < invIsAfCofficient.length; i++) {
invIsAfCofficient[i] = Af;
for (int j = 1; j < i; j++) {
invIsAfCofficient[i] = invIsAfCofficient[i].multiply(Af);
}
}
DoublePolynomialMatrix invIsAf = new DoublePolynomialMatrix(Af.getRowSize(), Af.getColumnSize());
for (int column = 1; column <= invIsAf.getColumnSize(); column++) {
for (int row = 1; row <= invIsAf.getRowSize(); row++) {
DoublePolynomial pol = new DoublePolynomial(new double[invIsAf.length()]);
for (int order = 0; order < invIsAf.length(); order++) {
pol.setCoefficient(order, invIsAfCofficient[order].getElement(row, column));
}
pol.simplify(tolerance);
invIsAf.setElement(row, column, pol);
}
}
DoubleRationalPolynomialMatrix Gf = new DoubleRationalPolynomialMatrix(invIsAf);
Gf = new DoubleRationalPolynomialMatrix(Cf).multiply(Gf).multiply(new DoubleRationalPolynomialMatrix(Bf));
DoubleRationalPolynomialMatrix G = Gs.subtract(Gf);
final DoubleRationalPolynomialMatrix simplifiedG = G.createClone();
for (int row = 1; row <= G.getRowSize(); row++) {
for (int column = 1; column <= G.getColumnSize(); column++) {
DoubleRationalPolynomial g = G.getElement(row, column);
DoublePolynomial numerator = g.getNumerator();
DoublePolynomial denominator = g.getDenominator();
if (simplify) {
numerator.simplify(tolerance);
denominator.simplify(tolerance);
}
simplifiedG.setElement(row, column, new DoubleRationalPolynomial(numerator, denominator));
}
}
simpG = simplifiedG;
return simpG;
}
/**
* @param A ゲイン行列
* @param B 入力行列
* @param C 出力行列
* @param D ゲイン行列
* @param E ディスクリプター行列
* @param tolerance 許容誤差
* @param simplify 簡単化するならばtrue
* @return TransferFunctionMatrix
* @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>> AnyRealRationalPolynomialMatrix<RS, RM, CS, CM> des2tfm(
RM A, RM B, RM C, RM D, RM E, RS tolerance, boolean simplify) {
String message;
if ((message = Abcdchk.abcdchk(A, B, C, D)).length() > 0) {
throw new RuntimeException(message);
}
List<RM> abcde = new ArrayList<>();
abcde.add(A);
abcde.add(B);
abcde.add(C);
abcde.add(D);
abcde.add(E);
List<RM> para = new WeierstrassForm<RS, RM, CS, CM>().weiestrassformByHessenberg(abcde, tolerance);
int zeroCount = 0;
for (int i = 1; i <= E.getColumnSize(); i++) {
if (para.get(4).getElement(i, i).isZero(tolerance)) {
zeroCount = i - 1;
break;
}
}
RM As = para.get(0).getSubMatrix(1, zeroCount, 1, zeroCount);
RM Af = para.get(4).getSubMatrix(1 + zeroCount, para.get(4).getColumnSize(), 1 + zeroCount, para.get(4).getColumnSize());
RM Bs = para.get(1).getSubMatrix(1, zeroCount, 1, para.get(1).getColumnSize());
RM Bf = para.get(1).getSubMatrix(1 + zeroCount, para.get(1).getRowSize(), 1, para.get(1).getColumnSize());
RM Cs = para.get(2).getSubMatrix(1, para.get(2).getRowSize(), 1, zeroCount);
RM Cf = para.get(2).getSubMatrix(1, para.get(2).getRowSize(), zeroCount + 1, para.get(2).getColumnSize());
AnyRealRationalPolynomialMatrix<RS, RM, CS, CM> Gs = Ss2tfm.ss2tfm(As, Bs, Cs, D);
List<RM> invIsAfCofficient = new ArrayList<>(Af.getColumnSize());
invIsAfCofficient.add(Af.createUnit());
for (int i = 1; i < invIsAfCofficient.size(); i++) {
invIsAfCofficient.add(Af);
for (int j = 1; j < i; j++) {
invIsAfCofficient.set(i, invIsAfCofficient.get(i).multiply(Af));
}
}
AnyRealPolynomial<RS, RM, CS, CM>[][] af = Gs.getElement(1, 1).getNumerator().createArray(Af.getRowSize(), Af.getColumnSize());
AnyRealPolynomialMatrix<RS, RM, CS, CM> invIsAf = new AnyRealPolynomialMatrix<>(Af.getRowSize(), Af.getColumnSize(), af);
for (int column = 1; column <= invIsAf.getColumnSize(); column++) {
for (int row = 1; row <= invIsAf.getRowSize(); row++) {
RS[] cc = tolerance.createArray(invIsAf.length());
AnyRealPolynomial<RS, RM, CS, CM> pol = new AnyRealPolynomial<>(cc);
for (int order = 0; order < invIsAf.length(); order++) {
pol.setCoefficient(order, invIsAfCofficient.get(order).getElement(row, column));
}
pol.simplify();
invIsAf.setElement(row, column, pol);
}
}
AnyRealRationalPolynomialMatrix<RS, RM, CS, CM> Gf = invIsAf.toRational();
Gf = new AnyRealPolynomialMatrix<>(Cf).toRational().multiply(Gf).multiply(new AnyRealPolynomialMatrix<>(Bf).toRational());
AnyRealRationalPolynomialMatrix<RS, RM, CS, CM> G = Gs.subtract(Gf);
final AnyRealRationalPolynomialMatrix<RS, RM, CS, CM> simplifiedG = G.createClone();
for (int row = 1; row <= G.getRowSize(); row++) {
for (int column = 1; column <= G.getColumnSize(); column++) {
AnyRealRationalPolynomial<RS, RM, CS, CM> g = G.getElement(row, column);
AnyRealPolynomial<RS, RM, CS, CM> numerator = g.getNumerator();
AnyRealPolynomial<RS, RM, CS, CM> denominator = g.getDenominator();
if (simplify) {
numerator.simplify(tolerance);
denominator.simplify(tolerance);
}
simplifiedG.setElement(row, column, new AnyRealRationalPolynomial<>(numerator, denominator));
}
}
AnyRealRationalPolynomialMatrix<RS, RM, CS, CM> simpG = simplifiedG;
return simpG;
}
/**
* @param A ゲイン行列
* @param B 入力行列
* @param C 出力行列
* @param D ゲイン行列
* @param E ディスクリプター行列
* @param simplify 簡単化するならばtrue
* @return TransferFunctionMatrix
* @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>> AnyRealRationalPolynomialMatrix<RS, RM, CS, CM> des2tfm(
RM A, RM B, RM C, RM D, RM E, boolean simplify) {
RM numericalA = A;
RS eps = numericalA.getElement(1, 1).getMachineEpsilon();
RS defaultTolerance = numericalA.frobNorm().multiply(eps).multiply(A.getRowSize());
return des2tfm(A, B, C, D, E, defaultTolerance, simplify);
}
}