WeierstrassForm.java
/*
* Created on 2010/10/19
* Copyright (C) 2010 Koga Laboratory. All rights reserved.
*
*/
package org.mklab.tool.control;
import java.util.ArrayList;
import java.util.List;
import org.mklab.nfc.eig.QRDecomposition;
import org.mklab.nfc.eig.RealQZDecomposition;
import org.mklab.nfc.matrix.ComplexNumericalMatrix;
import org.mklab.nfc.matrix.RealNumericalMatrix;
import org.mklab.nfc.scalar.ComplexNumericalScalar;
import org.mklab.nfc.scalar.RealNumericalScalar;
/**
* ディスクリプタシステム表現をそのワイエルストラス標準形に変換するクラスです。
*
* @author esumi
* @version $Revision$, 2010/10/19
* @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 class WeierstrassForm<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>> {
/** 右下にゼロを集めるために左からかける変換行列 */
private RM TL;
/** 右下にゼロを集めるために右からかける変換行列 */
private RM TR;
/** ブロック対角化するために左からかける変換行列 */
private RM BDL;
/** ブロック対角化するために右からかける変換行列 */
private RM BDR;
/** ヘッセンベルグ行列にするために左からかける変換行列 */
private RM HL;
/** ヘッセンベルグ行列にするために右からかける変換行列 */
private RM HR;
/**
* 行列Aをヘッセンベルグ行列,行列Eを上三角行列に変換して返します。
*
* @param A 変換前の行列
* @param E 変換前の行列
* @param tolerance 許容誤差
* @return 変換後の行列Aと行列E
*/
private List<RM> hessenbergForm(final RM A, final RM E, RS tolerance) {
this.HL = A.createUnit();
this.HR = A.createUnit();
RM tA = A.createClone();
RM tE = E.createClone();
QRDecomposition<RS, RM> qr = tE.qrDecompose();
this.HL = qr.getQ().transpose();
RM qrE = qr.getR();
RM qrA = qr.getQ().transpose().multiply(tA);
for (int i = 1; i < A.getColumnSize() - 1; i++) {
for (int j = A.getRowSize() - 1; j > i; j--) {
RS qa = qrA.getElement(j, i);
RS qb = qrA.getElement(j + 1, i);
RM Q = givens(qa, qb, tolerance);
this.HL.setSubMatrix(j, j + 1, 1, this.HL.getColumnSize(), Q.multiply(this.HL.getSubMatrix(j, j + 1, 1, this.HL.getColumnSize())));
qrE.setSubMatrix(j, j + 1, 1, qrE.getColumnSize(), Q.multiply(qrE.getSubMatrix(j, j + 1, 1, qrE.getColumnSize())));
qrA.setSubMatrix(j, j + 1, 1, qrA.getColumnSize(), Q.multiply(qrA.getSubMatrix(j, j + 1, 1, qrA.getColumnSize())));
RS za = qrE.getElement(j + 1, j + 1);
RS zb = qrE.getElement(j + 1, j);
RM Z = givens(za, zb, tolerance);
this.HR.setSubMatrix(1, this.HR.getRowSize(), j, j + 1, this.HR.getSubMatrix(1, this.HR.getRowSize(), j, j + 1).multiply(Z));
qrA.setSubMatrix(1, qrA.getRowSize(), j, j + 1, qrA.getSubMatrix(1, qrA.getRowSize(), j, j + 1).multiply(Z));
qrE.setSubMatrix(1, qrE.getRowSize(), j, j + 1, qrE.getSubMatrix(1, qrE.getRowSize(), j, j + 1).multiply(Z));
}
}
List<RM> ans = new ArrayList<>();
ans.add(qrA);
ans.add(qrE);
return ans;
}
/**
* 上三角行列Eの対角成分の右下にゼロを集めた行列とヘッセンベルグ行列Aにそれと同じ変換をした行列を返します。
*
* @param A 変換前の行列（上三角行列）
* @param E 変換後の行列（上三角行列）
* @param tolerance 許容誤差
* @return 変換後の行列Aと変換後の行列E
*/
private List<RM> hessenbergPushDownZero(final RM A, final RM E, RS tolerance) {
this.TL = A.createUnit();
this.TR = A.createUnit();
RM tA = A.createClone();
RM tE = E.createClone();
for (int i = 1; i <= tA.getColumnSize(); i++) {
for (int j = 1; j <= tA.getColumnSize() - i; j++) {
RS e11 = tE.getElement(j, j);
RS e22 = tE.getElement(j + 1, j + 1);
if (!e11.isZero(tolerance) || e22.isZero(tolerance)) {
continue;
}
RS q1e1 = tE.getElement(j, j + 1);
RS q1e2 = tE.getElement(j + 1, j + 1);
RM Q1 = givens(q1e1, q1e2, tolerance);
this.TL.setSubMatrix(j, j + 1, 1, this.TL.getColumnSize(), Q1.multiply(this.TL.getSubMatrix(j, j + 1, 1, this.TL.getColumnSize())));
tE.setSubMatrix(j, j + 1, 1, tE.getColumnSize(), Q1.multiply(tE.getSubMatrix(j, j + 1, 1, tE.getColumnSize())));
tA.setSubMatrix(j, j + 1, 1, tA.getColumnSize(), Q1.multiply(tA.getSubMatrix(j, j + 1, 1, tA.getColumnSize())));
if (j == 1) {
continue;
}
RS z1a1 = tA.getElement(j + 1, j);
RS z1a2 = tA.getElement(j + 1, j - 1);
RM Z1 = givens(z1a1, z1a2, tolerance);
this.TR.setSubMatrix(1, this.TR.getRowSize(), j - 1, j, this.TR.getSubMatrix(1, this.TR.getRowSize(), j - 1, j).multiply(Z1));
tA.setSubMatrix(1, tA.getRowSize(), j - 1, j, tA.getSubMatrix(1, tA.getRowSize(), j - 1, j).multiply(Z1));
tE.setSubMatrix(1, tE.getRowSize(), j - 1, j, tE.getSubMatrix(1, tE.getRowSize(), j - 1, j).multiply(Z1));
if (j < tA.getColumnSize() - i) {
continue;
}
RS z2a1 = tA.getElement(j + 1, j + 1);
RS z2a2 = tA.getElement(j + 1, j);
RM Z2 = givens(z2a1, z2a2, tolerance);
this.TR.setSubMatrix(1, this.TR.getRowSize(), j, j + 1, this.TR.getSubMatrix(1, this.TR.getRowSize(), j, j + 1).multiply(Z2));
tA.setSubMatrix(1, tA.getRowSize(), j, j + 1, tA.getSubMatrix(1, tA.getRowSize(), j, j + 1).multiply(Z2));
tE.setSubMatrix(1, tE.getRowSize(), j, j + 1, tE.getSubMatrix(1, tE.getRowSize(), j, j + 1).multiply(Z2));
}
}
for (int i = tE.getColumnSize(); i > 1; i--) {
if (tE.getElement(i, i).isZero(tolerance) && !tA.getElement(i, i - 1).isZero(tolerance)) {
RS z2a1 = tA.getElement(i, i);
RS z2a2 = tA.getElement(i, i - 1);
RM Z2 = givens(z2a1, z2a2, tolerance);
this.TR.setSubMatrix(1, this.TR.getRowSize(), i - 1, i, this.TR.getSubMatrix(1, this.TR.getRowSize(), i - 1, i).multiply(Z2));
tA.setSubMatrix(1, tA.getRowSize(), i - 1, i, tA.getSubMatrix(1, tA.getRowSize(), i - 1, i).multiply(Z2));
tE.setSubMatrix(1, tE.getRowSize(), i - 1, i, tE.getSubMatrix(1, tE.getRowSize(), i - 1, i).multiply(Z2));
}
}
List<RM> ans = new ArrayList<>();
ans.add(tA);
ans.add(tE);
return ans;
}
/**
* 上三角行列Eの対角成分の右下にゼロを集めた行列と上三角行列Aにそれと同じ変換をした行列を返します。
*
* @param A 変換前の行列（上三角行列）
* @param E 変換後の行列（上三角行列）
* @param tolerance 許容誤差
* @return 変換後の行列Aと変換後の行列E
*/
private List<RM> qzPushDownZero(final RM A, final RM E, RS tolerance) {
this.TL = A.createUnit();
this.TR = A.createUnit();
RM tA = A.createClone();
RM tE = E.createClone();
for (int i = 1; i <= tA.getColumnSize(); i++) {
for (int j = 1; j <= tA.getColumnSize() - i; j++) {
RS e11 = tE.getElement(j, j);
RS e22 = tE.getElement(j + 1, j + 1);
if (!e11.isZero(tolerance) || e22.isZero(tolerance)) {
continue;
}
boolean blockFlag = false;
if (j < tA.getColumnSize() - i) {
if (!tA.getElement(j + 2, j + 1).isZero(tolerance)) {
blockFlag = true;
}
}
if (!blockFlag) {
RS q1e1 = tE.getElement(j, j + 1);
RS q1e2 = tE.getElement(j + 1, j + 1);
RM Q1 = givens(q1e1, q1e2, tolerance);
this.TL.setSubMatrix(j, j + 1, 1, this.TL.getColumnSize(), Q1.multiply(this.TL.getSubMatrix(j, j + 1, 1, this.TL.getColumnSize())));
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())));
RS z1a1 = tA.getElement(j + 1, j + 1);
RS z1a2 = tA.getElement(j + 1, j);
RM Z1 = givens(z1a1, z1a2, tolerance);
this.TR.setSubMatrix(1, this.TR.getRowSize(), j, j + 1, this.TR.getSubMatrix(1, this.TR.getRowSize(), j, j + 1).multiply(Z1));
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));
} else {
RS q1e1 = tE.getElement(j, j + 1);
RS q1e2 = tE.getElement(j + 1, j + 1);
RM Q1 = givens(q1e1, q1e2, tolerance);
this.TL.setSubMatrix(j, j + 1, 1, this.TL.getColumnSize(), Q1.multiply(this.TL.getSubMatrix(j, j + 1, 1, this.TL.getColumnSize())));
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())));
RS q2e1 = tE.getElement(j + 1, j + 2);
RS q2e2 = tE.getElement(j + 2, j + 2);
RM Q2 = givens(q2e1, q2e2, tolerance);
this.TL.setSubMatrix(j + 1, j + 2, 1, this.TL.getColumnSize(), Q2.multiply(this.TL.getSubMatrix(j + 1, j + 2, 1, this.TL.getColumnSize())));
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())));
RS z1a1 = tA.getElement(j + 2, j + 1);
RS z1a2 = tA.getElement(j + 2, j);
RM Z1 = givens(z1a1, z1a2, tolerance);
this.TR.setSubMatrix(1, this.TR.getRowSize(), j, j + 1, this.TR.getSubMatrix(1, this.TR.getRowSize(), j, j + 1).multiply(Z1));
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));
RS z2a1 = tA.getElement(j + 2, j + 2);
RS z2a2 = tA.getElement(j + 2, j + 1);
RM Z2 = givens(z2a1, z2a2, tolerance);
this.TR.setSubMatrix(1, this.TR.getRowSize(), j + 1, j + 2, this.TR.getSubMatrix(1, this.TR.getRowSize(), j + 1, j + 2).multiply(Z2));
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));
j++;
}
}
}
List<RM> ans = new ArrayList<>();
ans.add(tA);
ans.add(tE);
return ans;
}
/**
* ギブンス回転行列を返します。
*
* @param a 数値1
* @param b 数値2
* @param tolerance 許容誤差
* @return ギブンス回転行列
*/
private RM givens(RS a, RS b, RS tolerance) {
final RS c;
final RS s;
if (b.isZero(tolerance)) {
c = a.create(1);
s = a.create(0);
} else if (a.isZero(tolerance)) {
c = a.create(0);
s = a.create(1);
} else if (a.abs().isLessThan(b.abs())) {
final RS tau = a.divide(b);
s = a.create(1).divide(tau.power(2).add(1).sqrt());
c = s.multiply(tau);
} else {
final RS tau = b.divide(a);
c = a.create(1).divide(tau.power(2).add(1).sqrt());
s = c.multiply(tau);
}
RS[][] g = s.createArray(2, 2);
g[0][0] = c;
g[0][1] = s;
g[1][0] = s.unaryMinus();
g[1][1] = c;
RM G = a.createGrid(2, 2, g);
return G;
}
/**
* 上三角行列の逆行列を求めます
*
* @param A 上三角行列
* @return Aの逆行列
*/
private RM upperTriInverse(RM A) {
RM B = A.createZero();
for (int i = 1; i <= A.getRowSize(); i++) {
B.setElement(i, i, A.getElement(i, i).inverse());
for (int j = i + 1; j <= A.getColumnSize(); j++) {
RS ba = A.getElement(i, i).create(0);
for (int k = 1; k <= j - 1; k++) {
ba = ba.add(B.getElement(i, k).multiply(A.getElement(k, j)));
}
B.setElement(i, j, A.getElement(j, j).inverse().multiply(ba).unaryMinus());
}
}
return B;
}
/**
* 下三角行列の逆行列を求めます
*
* @param A 下三角行列
* @return Aの逆行列
*/
private RM lowerTriInverse(RM A) {
RM B = A.createZero();
for (int i = A.getRowSize(); i >= 1; i--) {
B.setElement(i, i, A.getElement(i, i).inverse());
for (int j = i - 1; j >= 1; j--) {
RS ba = A.getElement(i, i).create(0);
for (int k = i; k >= j; k--) {
ba = ba.add(B.getElement(i, k).multiply(A.getElement(k, j)));
}
B.setElement(i, j, A.getElement(j, j).inverse().multiply(ba).unaryMinus());
}
}
return B;
}
/**
* QZ変換された行列Aと行列Eをブロック対角化した行列を返します。
*
* @param A QZ変換された行列A
* @param E QZ変換された行列E
* @param tolerance 許容誤差
* @return ブロック対角化した行列AとE
*/
private List<RM> blockDiagonaliize(final RM A, final RM E, RS tolerance) {
int zeroCount = 0;
for (int i = 1; i <= E.getColumnSize(); i++) {
if (E.getElement(i, i).isZero(tolerance)) {
zeroCount = i - 1;
break;
}
}
final RM A11 = A.getSubMatrix(1, zeroCount, 1, zeroCount);
final RM A12 = A.getSubMatrix(1, zeroCount, 1 + zeroCount, A.getColumnSize());
final RM A22 = A.getSubMatrix(1 + zeroCount, A.getColumnSize(), 1 + zeroCount, A.getColumnSize());
final RM E11 = E.getSubMatrix(1, zeroCount, 1, zeroCount);
final RM E12 = E.getSubMatrix(1, zeroCount, 1 + zeroCount, E.getColumnSize());
final RM E22 = E.getSubMatrix(1 + zeroCount, E.getColumnSize(), 1 + zeroCount, E.getColumnSize());
if (zeroCount == 0) {
this.BDL = upperTriInverse(A22);
this.BDR = A.createUnit();
RM bA = A22.createUnit();
//NumericalMatrixOperator<?> bE = (NumericalMatrixOperator<?>)upperTriInverse(A22).multiply(E22);
RM bE = A22.leftDivide(E22);
List<RM> ans = new ArrayList<>();
ans.add(bA);
ans.add(bE);
return ans;
} else if (zeroCount == A.getColumnSize()) {
this.BDL = upperTriInverse(E11);
this.BDR = A.createUnit();
//NumericalMatrixOperator<?> bA = (NumericalMatrixOperator<?>)upperTriInverse(E11).multiply(A11);
RM bA = E11.leftDivide(A11);
RM bE = E22.createUnit();
List<RM> ans = new ArrayList<>();
ans.add(bA);
ans.add(bE);
return ans;
} else {
int m = A11.getColumnSize();
int n = A22.getColumnSize();
final RM blockMatrix = A.createZero(m * n, m * n);
final RM invE11 = upperTriInverse(E11);
final RM 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);
}
final RM A11invE11 = A11.multiply(invE11);
final RM 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);
}
final RM 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().getElement(1 + row, 1 + column)));
}
}
final RM 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().getElement(1 + row, 1 + column)).subtract(step3.getSubMatrix(1 + row * m, (1 + row) * m, 1 + column * m, (1 + column) * m)));
}
}
final RM step5 = lowerTriInverse(step4);
final RM 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().getElement(1 + row, 1 + column)));
}
}
final RM step7 = step6.multiply(step5);
final RM step8 = step5.multiply(step2);
final RM step9 = step7.multiply(step2);
final RM step10 = step1.add(step9);
final RM step11 = A.createZero(n * m * 2, n * m * 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);
final RM step12 = A.createZero(n * m * 2, n * m * 2);
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));
}
final RM step13 = step11.multiply(step12);
final RM BDR12 = A12.createZero();
final RM 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, upperTriInverse(A22));
RM bA = A.createClone();
//bA.setSubMatrix(1, A11.getRowSize(), 1, A11.getColumnSize(), upperTriInverse(E11).multiply(A11));
bA.setSubMatrix(1, A11.getRowSize(), 1, A11.getColumnSize(), E11.leftDivide(A11));
bA.setSubMatrix(1, A11.getRowSize(), 1 + A11.getColumnSize(), A.getColumnSize(), A12.createZero());
bA.setSubMatrix(1 + A11.getRowSize(), A.getRowSize(), 1, A11.getColumnSize(), A12.transpose().createZero());
bA.setSubMatrix(1 + A11.getRowSize(), A.getRowSize(), 1 + A11.getColumnSize(), A.getColumnSize(), A22.createUnit());
RM bE = E.createClone();
bE.setSubMatrix(1, A11.getRowSize(), 1, A11.getColumnSize(), E11.createUnit());
bE.setSubMatrix(1, A11.getRowSize(), 1 + A11.getColumnSize(), A.getColumnSize(), A12.createZero());
bE.setSubMatrix(1 + A11.getRowSize(), A.getRowSize(), 1, A11.getColumnSize(), A12.transpose().createZero());
//bE.setSubMatrix(1 + A11.getRowSize(), A.getRowSize(), 1 + A11.getColumnSize(), A.getColumnSize(), upperTriInverse(A22).multiply(E22));
bE.setSubMatrix(1 + A11.getRowSize(), A.getRowSize(), 1 + A11.getColumnSize(), A.getColumnSize(), A22.leftDivide(E22));
List<RM> ans = new ArrayList<>();
ans.add(bA);
ans.add(bE);
return ans;
}
}
/**
* ディスクリプタシステム表現をQZ分解を使ってそのワイエルストラス標準形に変換します
*
* @param para 変換前のA,B,C,D,Eが格納された配列
* @param tolerance 許容誤差
* @return 変換後のA,B,C,D,Eが格納された配列
*/
public List<RM> weiestrassformByQZ(List<RM> para, RS tolerance) {
final RealQZDecomposition<RS, RM, CS, CM> aeqz = para.get(0).qzDecompose(para.get(4));
final List<RM> tae = qzPushDownZero(aeqz.getAA(), aeqz.getBB(), tolerance);
final List<RM> bae = blockDiagonaliize(tae.get(0), tae.get(1), tolerance);
final RM P = this.BDL.multiply(this.TL).multiply(aeqz.getQ().conjugateTranspose());
final RM Q = aeqz.getZ().multiply(this.TR).multiply(this.BDR);
final RM A = bae.get(0);
final RM B = P.multiply(para.get(1));
final RM C = para.get(2).multiply(Q);
final RM D = para.get(3);
final RM E = bae.get(1);
List<RM> abcde = new ArrayList<>();
abcde.add(A);
abcde.add(B);
abcde.add(C);
abcde.add(D);
abcde.add(E);
return abcde;
}
/**
* ディスクリプタシステム表現を一般化ヘッセンベルグ形式を使ってそのワイエルストラス標準形に変換します
*
* @param para 変換前のA,B,C,D,Eが格納された配列
* @param tolerance 許容誤差
* @return 変換後のA,B,C,D,Eが格納された配列
*/
public List<RM> weiestrassformByHessenberg(List<RM> para, RS tolerance) {
final List<RM> qae = hessenbergForm(para.get(0), para.get(4), tolerance);
final List<RM> tae = hessenbergPushDownZero(qae.get(0), qae.get(1), tolerance);
final List<RM> bae2 = blockDiagonaliize2(tae.get(0), tae.get(1), tolerance);
// final RM[] bae = blockDiagonaliize(tae[0], tae[1], tolerance);
final RM P = this.BDL.multiply(this.TL).multiply(this.HL);
final RM Q = this.HR.multiply(this.TR).multiply(this.BDR);
final RM A = bae2.get(0);
final RM B = P.multiply(para.get(1));
final RM C = para.get(2).multiply(Q);
final RM D = para.get(3);
final RM E = bae2.get(1);
List<RM> abcde = new ArrayList<>();
abcde.add(A);
abcde.add(B);
abcde.add(C);
abcde.add(D);
abcde.add(E);
return abcde;
}
private List<RM> blockDiagonaliize2(RM A, RM E, RS tolerance) {
int zeroCount = 0;
for (int i = 1; i <= E.getColumnSize(); i++) {
if (E.getElement(i, i).isZero(tolerance)) {
zeroCount = i - 1;
break;
}
}
final RM A11 = A.getSubMatrix(1, zeroCount, 1, zeroCount);
final RM A12 = A.getSubMatrix(1, zeroCount, 1 + zeroCount, A.getColumnSize());
final RM A22 = A.getSubMatrix(1 + zeroCount, A.getColumnSize(), 1 + zeroCount, A.getColumnSize());
final RM E11 = E.getSubMatrix(1, zeroCount, 1, zeroCount);
final RM E12 = E.getSubMatrix(1, zeroCount, 1 + zeroCount, E.getColumnSize());
final RM E22 = E.getSubMatrix(1 + zeroCount, E.getColumnSize(), 1 + zeroCount, E.getColumnSize());
if (zeroCount == 0) {
this.BDL = upperTriInverse(A22);
this.BDR = A.createUnit();
RM bA = A22.createUnit();
//NumericalMatrixOperator<?> bE = (NumericalMatrixOperator<?>)upperTriInverse(A22).multiply(E22);
RM bE = A22.leftDivide(E22);
List<RM> ans = new ArrayList<>();
ans.add(bA);
ans.add(bE);
return ans;
} else if (zeroCount == A.getColumnSize()) {
this.BDL = upperTriInverse(E11);
this.BDR = A.createUnit();
//NumericalMatrixOperator<?> bA = (NumericalMatrixOperator<?>)upperTriInverse(E11).multiply(A11);
RM bA = E11.leftDivide(A11);
RM bE = E22.createUnit();
List<RM> ans = new ArrayList<>();
ans.add(bA);
ans.add(bE);
return ans;
} else {
int m = A11.getColumnSize();
int n = A22.getColumnSize();
final List<RM> GSE_ans = solveGSE_HT(A11, A12, A22, E11, E12, E22);
final RM invE11 = upperTriInverse(E11);
this.BDR = A.createUnit();
this.BDL = A.createUnit();
this.BDR.setSubMatrix(1, m, m + 1, m + n, GSE_ans.get(1));
this.BDL.setSubMatrix(1, m, 1, m, invE11);
this.BDL.setSubMatrix(1, m, m + 1, m + n, invE11.multiply(GSE_ans.get(0)));
this.BDL.setSubMatrix(m + 1, m + n, m + 1, m + n, upperTriInverse(A22));
RM bA = A.createClone();
//bA.setSubMatrix(1, A11.getRowSize(), 1, A11.getColumnSize(), upperTriInverse(E11).multiply(A11));
bA.setSubMatrix(1, A11.getRowSize(), 1, A11.getColumnSize(), E11.leftDivide(A11));
bA.setSubMatrix(1, A11.getRowSize(), 1 + A11.getColumnSize(), A.getColumnSize(), A12.createZero());
bA.setSubMatrix(1 + A11.getRowSize(), A.getRowSize(), 1, A11.getColumnSize(), A12.transpose().createZero());
bA.setSubMatrix(1 + A11.getRowSize(), A.getRowSize(), 1 + A11.getColumnSize(), A.getColumnSize(), A22.createUnit());
RM bE = E.createClone();
bE.setSubMatrix(1, A11.getRowSize(), 1, A11.getColumnSize(), E11.createUnit());
bE.setSubMatrix(1, A11.getRowSize(), 1 + A11.getColumnSize(), A.getColumnSize(), A12.createZero());
bE.setSubMatrix(1 + A11.getRowSize(), A.getRowSize(), 1, A11.getColumnSize(), A12.transpose().createZero());
//bE.setSubMatrix(1 + A11.getRowSize(), A.getRowSize(), 1 + A11.getColumnSize(), A.getColumnSize(), upperTriInverse(A22).multiply(E22));
bE.setSubMatrix(1 + A11.getRowSize(), A.getRowSize(), 1 + A11.getColumnSize(), A.getColumnSize(), A22.leftDivide(E22));
List<RM> ans = new ArrayList<>();
ans.add(bA);
ans.add(bE);
return ans;
}
}
/**
* { A_11 R + L A_22 = -A_12 { E_11 R + L E_22 = -E_12 iff [I L][A_11 A_12][I R] = [A_11 0] [0 I][0 A_22][0 I] = [0 A_22] [I L][E_11 E_12][I R] = [E_11 0] [0 I][0 E_22][0 I] = [0 E_22]
* A_11:上ヘッセンベルグ行列,A_22:(対角成分が非0)上三角行列 E_11:(対角成分が非0)上三角行列,E_22上べき零行列 を満たす一般化シルベスター方程式を解く
*
* @param A11 A11
* @param A12 A12
* @param A22 A22
* @param E11 E11
* @param E12 E12
* @param E22 E22
* @return answer 答
*/
private List<RM> solveGSE_HT(final RM A11, final RM A12, final RM A22, final RM E11, final RM E12, final RM E22) {
int m = A11.getColumnSize();
int n = A22.getColumnSize();
final RM R = A11.createZero(m, n);
final RM L = A11.createZero(m, n);
final RM H = A11.createZero(m, n);
final RM G = A11.createZero(m, n);
final RM H_Dr = A11.createZero(m, n);
final RM G_Ar = A11.createZero(m, n);
for (int k = 1; k < n + 1; k++) {
H.setColumnVector(k, E12.getColumnVector(k).unaryMinus());
for (int l = m; l > 0; l--) {
for (int i = 1; i < k; i++) {
H.setSubMatrix(l, l, k, k, H.getSubMatrix(l, l, k, k).subtract(L.getSubMatrix(l, l, i, i).multiply(E22.getSubMatrix(i, i, k, k))));
}
H_Dr.setSubMatrix(l, l, k, k, H.getSubMatrix(l, l, k, k));
for (int j = l + 1; j < m + 1; j++) {
H_Dr.setSubMatrix(l, l, k, k, H_Dr.getSubMatrix(l, l, k, k).subtract(E11.getSubMatrix(l, l, j, j).multiply(R.getSubMatrix(j, j, k, k))));
}
R.setSubMatrix(l, l, k, k, H_Dr.getSubMatrix(l, l, k, k).divide(E11.getSubMatrix(l, l, l, l)));
}
G.setColumnVector(k, A12.getColumnVector(k).unaryMinus());
for (int l = m; l > 0; l--) {
for (int i = 1; i < k; i++) {
G.setSubMatrix(l, l, k, k, G.getSubMatrix(l, l, k, k).subtract(L.getSubMatrix(l, l, i, i).multiply(A22.getSubMatrix(i, i, k, k))));
}
G_Ar.setSubMatrix(l, l, k, k, G.getSubMatrix(l, l, k, k));
if (l == 1) {
for (int i = 1; i < m + 1; i++) {
G_Ar.setSubMatrix(1, 1, k, k, G_Ar.getSubMatrix(1, 1, k, k).subtract(A11.getSubMatrix(1, 1, i, i).multiply(R.getSubMatrix(i, i, k, k))));
}
} else {
for (int j = l - 1; j < m + 1; j++) {
G_Ar.setSubMatrix(l, l, k, k, G_Ar.getSubMatrix(l, l, k, k).subtract(A11.getSubMatrix(l, l, j, j).multiply(R.getSubMatrix(j, j, k, k))));
}
}
L.setSubMatrix(l, l, k, k, G_Ar.getSubMatrix(l, l, k, k).divide(A22.getSubMatrix(k, k, k, k)));
}
}
List<RM> answer = new ArrayList<>();
answer.add(L);
answer.add(R);
return answer;
}
}