Balreal.java

/*
 * $Id: Balreal.java,v 1.27 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.eig.DoubleEigenSolution;
import org.mklab.nfc.matrix.DoubleComplexMatrix;
import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.matrix.IndexedMatrix;
import org.mklab.nfc.matrix.IntMatrix;
import org.mklab.nfc.scalar.DoubleComplexNumber;
import org.mklab.nfc.scalar.DoubleNumber;
import org.mklab.tool.matrix.Chol;
import org.mklab.tool.matrix.Tril;


/**
 * 連続時間系の平衡実現を求めるクラスです。
 * 
 * <p>Continuous-time balanced realization
 * 
 * @author koga
 * @version $Revision: 1.27 $
 * @see org.mklab.tool.control.Minreal
 * @see org.mklab.tool.control.Dbalreal
 */
public class Balreal {

  /**
   * メインメソッド
   * 
   * @param args コマンドライン引数
   */
  public static void main(String[] args) {
    DoubleMatrix a = new DoubleMatrix(new double[][] { {0, 1}, {-2, -3}});
    DoubleMatrix b = new DoubleMatrix(new double[][] { {0}, {1}});
    DoubleMatrix c = new DoubleMatrix(new double[] {5, 6});
    List<DoubleMatrix> tmp = Balreal.balreal(a, b, c);
    tmp.get(0).print("a"); //$NON-NLS-1$
    tmp.get(1).print("b"); //$NON-NLS-1$
    tmp.get(2).print("c"); //$NON-NLS-1$
  }

  /**
   * 平衡実現されたシステム(Ab, Bb, Cb)を返します。
   * 
   * <p>同時に、グラミアンの対角成分を含むベクトル <code>G</code> と同時変換行列 <code>T</code> を返します。
   * 
   * @param A 元のA行列
   * @param B 元のB行列
   * @param C 元のD行列
   * @return Ab, Bb, Cb, G, T
   */
  @SuppressWarnings("cast")
  public static List<DoubleMatrix> balreal(DoubleMatrix A, DoubleMatrix B, DoubleMatrix C) {
    String message;
    if ((message = Abcdchk.abcdchk(A, B, C)).length() > 0) {
      throw new IllegalArgumentException(message);
    }

    DoubleMatrix Gc = Gramian.gramian(A, B);
    DoubleMatrix Go = Gramian.gramian(A.conjugateTranspose(), C.conjugateTranspose());
    DoubleMatrix R = Chol.chol(Gc);
    DoubleMatrix rGr = R.multiply(Go).multiply(R.conjugateTranspose());
    rGr = Tril.tril(rGr).add(Tril.tril(rGr, -1).conjugateTranspose());

    DoubleEigenSolution tmp = ((DoubleMatrix)rGr).eigenDecompose();
    DoubleMatrix Dre = tmp.getRealValue();
    DoubleMatrix Dim = tmp.getImagValue();
    DoubleComplexMatrix D = new DoubleComplexMatrix(Dre,Dim);
    DoubleMatrix Vre = tmp.getRealVector();
    DoubleMatrix Vim = tmp.getImagVector();
    DoubleComplexMatrix V = new DoubleComplexMatrix(Vre,Vim);

    DoubleNumber unit = A.getElement(1, 1);
    DoubleNumber power = unit.createUnit().divide(4).unaryMinus();

    DoubleMatrix T = new DoubleComplexMatrix(R).conjugateTranspose().multiply(V).multiply((D.diagonalToVector()).powerElementWise(new DoubleComplexNumber(power)).vectorToDiagonal()).getRealPart();
    DoubleMatrix Ab = T.leftDivide(A).multiply(T);
    DoubleMatrix Bb = T.leftDivide(B);
    DoubleMatrix Cb = C.multiply(T);
    DoubleMatrix G = Gramian.gramian(Ab, Bb).diagonalToVector().conjugateTranspose();

    // G becomes in descending order
    IndexedMatrix<?,?> tmp2 = ((DoubleMatrix)G).sort();
    // DoubleMatrix GG = tmp[0];
    IntMatrix idx = (IntMatrix)(tmp2.getIndices());
    idx = (IntMatrix)(idx.flipLeftRight());
    Ab = Ab.getSubMatrix(idx, idx);
    Bb = Bb.getSubMatrix(idx, 1, Bb.getColumnSize());
    Cb = Cb.getSubMatrix(1, Cb.getRowSize(), idx);

    T = T.getSubMatrix(idx, idx);
    G = G.getSubVector(idx);

    return new ArrayList<>(Arrays.asList(new DoubleMatrix[] {Ab, Bb, Cb, G, T}));
  }

}