DoubleWeierstrassForm.java
/*
* Created on 2010/10/19
* Copyright (C) 2010 Koga Laboratory. All rights reserved.
*
*/
package org.mklab.tool.control;
import org.mklab.nfc.eig.RealQZDecomposition;
import org.mklab.nfc.matrix.DoubleComplexMatrix;
import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.scalar.DoubleComplexNumber;
import org.mklab.nfc.scalar.DoubleNumber;
/**
* ディスクリプタシステム表現をそのワイエルストラス標準形に変換するクラスです。
*
* @author esumi
* @version $Revision$, 2010/10/19
*/
public class DoubleWeierstrassForm {
/** 右下にゼロを集めるために左からかける変換行列 */
private DoubleMatrix TL;
/** 右下にゼロを集めるために右からかける変換行列 */
private DoubleMatrix TR;
/** ブロック対角化するために左からかける変換行列 */
private DoubleMatrix BDL;
/** ブロック対角化するために右からかける変換行列 */
private DoubleMatrix BDR;
/**
* @return bDLを返します。
*/
public DoubleMatrix getBDL() {
return this.BDL;
}
/**
* @return bDRを返します。
*/
public DoubleMatrix getBDR() {
return this.BDR;
}
/**
* 上三角行列Eの対角成分の右下にゼロを集めた行列と上三角行列Aにそれと同じ変換をした行列を返します。
*
* @param A 変換前の行列（上三角行列）
* @param E 変換後の行列（上三角行列）
* @return 変換後の行列Aと変換後の行列E
*/
DoubleMatrix[] collectZeroInLowerRight(final DoubleMatrix A, final DoubleMatrix E) {
this.TL = A.createUnit();
this.TR = A.createUnit();
DoubleMatrix tA = A.createClone();
DoubleMatrix tE = E.createClone();
for (int i = 1; i < tA.getColumnSize(); i++) {
for (int j = 1; j <= tA.getColumnSize() - i; j++) {
double e11 = tE.getDoubleElement(j, j);
double e12 = tE.getDoubleElement(j, j + 1);
double e22 = tE.getDoubleElement(j + 1, j + 1);
if (e11 != 0 || e22 == 0) {
continue;
}
boolean blockFlag = false;
if (j < tA.getColumnSize() - i) {
if (tA.getDoubleElement(j + 2, j + 1) != 0) {
blockFlag = true;
}
}
if (!blockFlag) {
double a11 = tA.getDoubleElement(j, j);
double a12 = tA.getDoubleElement(j, j + 1);
double a22 = tA.getDoubleElement(j + 1, j + 1);
DoubleMatrix P = tA.createZero(2, 2);
P.setElement(1, 1, a22 * e12 - a12 * e22);
P.setElement(1, 2, e22 * a11);
P.setElement(2, 1, e22 * a11);
P.setElement(2, 2, -e12 * a11);
tA.setSubMatrix(j, j + 1, j, tA.getColumnSize(), P.multiply(tA.getSubMatrix(j, j + 1, j, tA.getColumnSize())));
tA.setSubMatrix(1, j + 1, j, j + 1, tA.getSubMatrix(1, j + 1, j, j + 1).multiply(P));
tE.setSubMatrix(j, j + 1, j, tE.getColumnSize(), P.multiply(tE.getSubMatrix(j, j + 1, j, tE.getColumnSize())));
tE.setSubMatrix(1, j + 1, j, j + 1, tE.getSubMatrix(1, j + 1, j, j + 1).multiply(P));
this.TL.setSubMatrix(j, j + 1, 1, this.TL.getColumnSize(), P.multiply(this.TL.getSubMatrix(j, j + 1, 1, this.TL.getColumnSize())));
this.TR.setSubMatrix(1, this.TR.getRowSize(), j, j + 1, this.TR.getSubMatrix(1, this.TR.getRowSize(), j, j + 1).multiply(P));
}
else {
double q1e1 = tE.getDoubleElement(j, j + 1);
double q1e2 = tE.getDoubleElement(j + 1, j + 1);
double q1r = Math.sqrt(q1e1 * q1e1 + q1e2 * q1e2);
double q1c = q1e1 / q1r;
double q1s = q1e2 / q1r;
DoubleMatrix Q1 = new DoubleMatrix(new double[][] { {q1c, q1s}, {-q1s, q1c}});
tE.setSubMatrix(j, j + 1, j, tE.getColumnSize(), Q1.multiply(tE.getSubMatrix(j, j + 1, j, tE.getColumnSize())));
tA.setSubMatrix(j, j + 1, j, tA.getColumnSize(), Q1.multiply(tA.getSubMatrix(j, j + 1, j, tA.getColumnSize())));
this.TL.setSubMatrix(j, j + 1, 1, this.TL.getColumnSize(), Q1.multiply(this.TL.getSubMatrix(j, j + 1, 1, this.TL.getColumnSize())));
double q2e1 = tE.getDoubleElement(j + 1, j + 2);
double q2e2 = tE.getDoubleElement(j + 2, j + 2);
double q2r = Math.sqrt(q2e1 * q2e1 + q2e2 * q2e2);
double q2c = q2e1 / q2r;
double q2s = q2e2 / q2r;
DoubleMatrix Q2 = new DoubleMatrix(new double[][] { {q2c, q2s}, {-q2s, q2c}});
tE.setSubMatrix(j + 1, j + 2, j, tE.getColumnSize(), Q2.multiply(tE.getSubMatrix(j + 1, j + 2, j, tE.getColumnSize())));
tA.setSubMatrix(j + 1, j + 2, j, tA.getColumnSize(), Q2.multiply(tA.getSubMatrix(j + 1, j + 2, j, tA.getColumnSize())));
this.TL.setSubMatrix(j + 1, j + 2, 1, this.TL.getColumnSize(), Q2.multiply(this.TL.getSubMatrix(j + 1, j + 2, 1, this.TL.getColumnSize())));
double z1a1 = tA.getDoubleElement(j + 2, j);
double z1a2 = tA.getDoubleElement(j + 2, j + 1);
double z1r = Math.sqrt(z1a1 * z1a1 + z1a2 * z1a2);
double z1c = z1a2 / z1r;
double z1s = z1a1 / z1r;
DoubleMatrix Z1 = new DoubleMatrix(new double[][] { {z1c, z1s}, {-z1s, z1c}});
tA.setSubMatrix(1, tA.getRowSize(), j, j + 1, tA.getSubMatrix(1, tA.getRowSize(), j, j + 1).multiply(Z1));
tE.setSubMatrix(1, tE.getRowSize(), j, j + 1, tE.getSubMatrix(1, tE.getRowSize(), j, j + 1).multiply(Z1));
this.TR.setSubMatrix(1, this.TR.getRowSize(), j, j + 1, this.TR.getSubMatrix(1, this.TR.getRowSize(), j, j + 1).multiply(Z1));
double z2a1 = tA.getDoubleElement(j + 2, j + 1);
double z2a2 = tA.getDoubleElement(j + 2, j + 2);
double z2r = Math.sqrt(z2a1 * z2a1 + z2a2 * z2a2);
double z2c = z2a2 / z2r;
double z2s = z2a1 / z2r;
DoubleMatrix Z2 = new DoubleMatrix(new double[][] { {z2c, z2s}, {-z2s, z2c}});
tA.setSubMatrix(1, tA.getRowSize(), j + 1, j + 2, tA.getSubMatrix(1, tA.getRowSize(), j + 1, j + 2).multiply(Z2));
tE.setSubMatrix(1, tE.getRowSize(), j + 1, j + 2, tE.getSubMatrix(1, tE.getRowSize(), j + 1, j + 2).multiply(Z2));
this.TR.setSubMatrix(1, this.TR.getRowSize(), j + 1, j + 2, this.TR.getSubMatrix(1, this.TR.getRowSize(), j + 1, j + 2).multiply(Z2));
j++;
}
}
}
return new DoubleMatrix[] {tA, tE};
}
/**
* QZ変換された行列をブロック対角化する行列を計算します
*
* @param A QZ変換された行列A
* @param E QZ変換された行列E
*/
void calcBlockDiagonaliizeMatrix(final DoubleMatrix A, final DoubleMatrix E) {
int zeroCount = 0;
for (int i = 1; i <= E.getColumnSize(); i++) {
if (Math.abs(E.getDoubleElement(i, i)) < 1.0e-15) {
zeroCount = i - 1;
break;
}
}
DoubleMatrix A11 = A.getSubMatrix(1, zeroCount, 1, zeroCount);
DoubleMatrix A12 = A.getSubMatrix(1, zeroCount, 1 + zeroCount, A.getColumnSize());
DoubleMatrix A22 = A.getSubMatrix(1 + zeroCount, A.getColumnSize(), 1 + zeroCount, A.getColumnSize());
DoubleMatrix E11 = E.getSubMatrix(1, zeroCount, 1, zeroCount);
DoubleMatrix E12 = E.getSubMatrix(1, zeroCount, 1 + zeroCount, E.getColumnSize());
DoubleMatrix E22 = E.getSubMatrix(1 + zeroCount, E.getColumnSize(), 1 + zeroCount, E.getColumnSize());
if (zeroCount == 0) {
this.BDL = A22.inverse();
this.BDR = A.createUnit();
} else if (zeroCount == A.getColumnSize()) {
this.BDL = E11.inverse();
this.BDR = A.createUnit();
} else {
int m = A11.getColumnSize();
int n = A22.getColumnSize();
DoubleMatrix blockMatrix = new DoubleMatrix(m * n, m * n);
//step1
DoubleMatrix invE11 = E11.inverse();
DoubleMatrix step1 = blockMatrix.createZero();
for (int i = 0; i < n; i++) {
step1.setSubMatrix(1 + i * m, (i + 1) * m, 1 + i * m, (i + 1) * m, invE11);
}
//step2
DoubleMatrix A11invE11 = A11.multiply(invE11);
DoubleMatrix step2 = blockMatrix.createZero();
for (int i = 0; i < n; i++) {
step2.setSubMatrix(1 + i * m, (i + 1) * m, 1 + i * m, (i + 1) * m, A11invE11);
}
//step3
DoubleMatrix step3 = blockMatrix.createZero();
for (int column = 0; column < n; column++) {
for (int row = column + 1; row < n; row++) {
step3.setSubMatrix(1 + row * m, (1 + row) * m, 1 + column * m, (1 + column) * m, A11invE11.multiply(E22.transpose().getDoubleElement(1 + row, 1 + column)));
}
}
//step4
DoubleMatrix step4 = blockMatrix.createZero();
for (int column = 0; column < n; column++) {
for (int row = column; row < n; row++) {
step4.setSubMatrix(1 + row * m, (1 + row) * m, 1 + column * m, (1 + column) * m,
A11.createUnit().multiply(A22.transpose().getDoubleElement(1 + row, 1 + column)).subtract(step3.getSubMatrix(1 + row * m, (1 + row) * m, 1 + column * m, (1 + column) * m)));
}
}
//step5
DoubleMatrix step5 = step4.inverse();
//step6
DoubleMatrix step6 = blockMatrix.createZero();
for (int column = 0; column < n; column++) {
for (int row = column + 1; row < n; row++) {
step6.setSubMatrix(1 + row * m, (1 + row) * m, 1 + column * m, (1 + column) * m, invE11.multiply(E22.transpose().getDoubleElement(1 + row, 1 + column)));
}
}
DoubleMatrix step7 = step6.multiply(step5);
DoubleMatrix step8 = step5.multiply(step2);
DoubleMatrix step9 = step7.multiply(step2);
DoubleMatrix step10 = step1.add(step9);
//step11
DoubleMatrix step11 = new DoubleMatrix(m * n * 2, m * n * 2);
step11.setSubMatrix(1, n * m, 1, n * m, step10);
step11.setSubMatrix(1, n * m, n * m + 1, n * m * 2, step7.multiply(-1));
step11.setSubMatrix(1 + n * m, n * m * 2, 1, n * m, step8.multiply(-1));
step11.setSubMatrix(n * m + 1, n * m * 2, n * m + 1, n * m * 2, step5);
//step12
DoubleMatrix step12 = new DoubleMatrix(m * n * 2, 1);
for (int i = 0; i < n; i++) {
step12.setSubMatrix(1 + i * m, (1 + i) * m, 1, 1, E12.getColumnVector(1 + i).multiply(-1));
}
for (int i = 0; i < n; i++) {
step12.setSubMatrix(1 + (i + n) * m, (1 + i + n) * m, 1, 1, A12.getColumnVector(1 + i).multiply(-1));
}
//step13
DoubleMatrix step13 = step11.multiply(step12);
DoubleMatrix BDR12 = A12.createZero();
DoubleMatrix BDL12 = A12.createZero();
for (int i = 0; i < n; i++) {
BDR12.setColumnVector(1 + i, step13.getSubMatrix(1 + i * m, (1 + i) * m, 1, 1));
}
for (int i = 0; i < n; i++) {
BDL12.setColumnVector(1 + i, step13.getSubMatrix(1 + (i + n) * m, (1 + i + n) * m, 1, 1));
}
this.BDR = A.createUnit();
this.BDL = A.createUnit();
this.BDR.setSubMatrix(1, m, m + 1, m + n, BDR12);
this.BDL.setSubMatrix(1, m, 1, m, invE11);
this.BDL.setSubMatrix(1, m, m + 1, m + n, invE11.multiply(BDL12));
this.BDL.setSubMatrix(m + 1, m + n, m + 1, m + n, A22.inverse());
}
}
/**
* ディスクリプタシステム表現をそのワイエルストラス標準形に変換します
*
* @param para 変換前のA,B,C,D,Eが格納された配列
* @return 変換後のA,B,C,D,Eが格納された配列
*/
public DoubleMatrix[] weiestrassform(DoubleMatrix[] para) {
final RealQZDecomposition<DoubleNumber,DoubleMatrix,DoubleComplexNumber,DoubleComplexMatrix> aeqz = para[0].qzDecompose(para[4]);
final DoubleMatrix[] ae = collectZeroInLowerRight(aeqz.getAA(), aeqz.getBB());
final DoubleMatrix aa = ae[0];
final DoubleMatrix ee = ae[1];
calcBlockDiagonaliizeMatrix(aa, ee);
final DoubleMatrix P = this.BDL.multiply(this.TL).multiply(aeqz.getQ().conjugateTranspose());
final DoubleMatrix Q = aeqz.getZ().multiply(this.TR).multiply(this.BDR);
final DoubleMatrix A = P.multiply(para[0]).multiply(Q);
final DoubleMatrix B = P.multiply(para[1]);
final DoubleMatrix C = para[2].multiply(Q);
final DoubleMatrix D = para[3];
final DoubleMatrix E = P.multiply(para[4]).multiply(Q);
return new DoubleMatrix[] {A, B, C, D, E};
}
}