SkeletonDes2tf.java

/*
 * Created on 2009/06/16
 * 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.DoubleMatrix;
import org.mklab.nfc.matrix.IntMatrix;
import org.mklab.nfc.scalar.DoubleNumber;
import org.mklab.nfc.scalar.DoubleNumberUtil;
import org.mklab.nfc.svd.SingularValueDecomposition;
import org.mklab.tool.matrix.Diag;


/**
 * @author Anan
 * 
 */
public class SkeletonDes2tf {

  /**
   * @param A ゲイン行列
   * @param B 入力行列
   * @param C 出力行列
   * @param D ゲイン行列
   * @param E ディスクリプター行列
   * @return {分子, 分母}
   */
  public static List<DoubleMatrix> des2tf(final DoubleMatrix A, final DoubleMatrix B, final DoubleMatrix C, final DoubleMatrix D, final DoubleMatrix E) {
    System.out.println("This algorithm that calculate descriptor to transfer function is defectiveness"); //$NON-NLS-1$
    final SingularValueDecomposition<DoubleNumber,DoubleMatrix> udv = E.singularValueDecompose();
    final DoubleMatrix u = udv.getU();
    //final DoubleMatrix d = udv.getD();
    final DoubleMatrix v = udv.getV();

    final int rank = E.rank();
    final DoubleMatrix d11 = E.singularValue().getSubVector(1, rank).inverseElementWise().vectorToDiagonal();
    final int size = E.getRowSize() - rank;
    final DoubleMatrix d22 = DoubleMatrix.unit(size);
    final DoubleMatrix dDager = Diag.diag(d11, d22);
    final DoubleMatrix E2 = dDager.multiply(u.transpose()).multiply(E).multiply(v);
    final DoubleMatrix A2 = dDager.multiply(u.transpose()).multiply(A).multiply(v);
    final DoubleMatrix B2 = dDager.multiply(u.transpose().multiply(B));
    final DoubleMatrix C2 = C.multiply(v);

    //    u.multiply(d).multiply(v.transpose()).print("U*D*V'"); //$NON-NLS-1$
    //    
    //    dDager.print("Ddager"); //$NON-NLS-1$
    //    E2.print("Ddager*U'*E*V"); //$NON-NLS-1$
    //    A2.print("Ddager*U'*A*V"); //$NON-NLS-1$
    //    B2.print("Ddager*U'*B"); //$NON-NLS-1$
    //    C2.print("C*V"); //$NON-NLS-1$

    final List<DoubleMatrix> controlableABCD = Canon.canon(A2, B2, C2, D.createZero(1), "controllable"); //$NON-NLS-1$
    final DoubleMatrix Ab = controlableABCD.get(0);
    final DoubleMatrix Bb = controlableABCD.get(1);
    final DoubleMatrix Cb = controlableABCD.get(2);
    final DoubleMatrix Db = controlableABCD.get(3);
    final DoubleMatrix T = controlableABCD.get(4);
    final DoubleMatrix Eb = T.inverse().multiply(E2).multiply(T);

    final DoubleMatrix reversDenominator = Ab.getRowVector(Ab.getRowSize()).unaryMinus();
    final DoubleMatrix fullDenominator = reversDenominator.flipLeftRight();
    final DoubleMatrix denominator = stripLeadingZeros(fullDenominator, "denominator"); //$NON-NLS-1$

    final DoubleMatrix reversNumerator = Cb.getRowVector(Cb.getRowSize());
    final DoubleMatrix fullNumerator = (reversNumerator.add(D.multiply(reversDenominator).resize(reversNumerator.getRowSize(), reversNumerator.getColumnSize()))).flipLeftRight();
    final DoubleMatrix numerator = stripLeadingZeros(fullNumerator, "numerator"); //$NON-NLS-1$

    //    denominator.resize(numerator.getRowSize(), numerator.getColumnSize()).print();
    boolean debugFlag = false;
    if (debugFlag == true) {
      Ab.print("Ab"); //$NON-NLS-1$
      Bb.print("Bb"); //$NON-NLS-1$
      Cb.print("Cb"); //$NON-NLS-1$
      Db.print("Db"); //$NON-NLS-1$
      T.print("T"); //$NON-NLS-1$
      Eb.print("Eb"); //$NON-NLS-1$

      T.print("T"); //$NON-NLS-1$
    }

    //    numerator.print("numerator");
    //    denominator.print("denominator");

    DoubleMatrix Ti = T.inverse();

    if (debugFlag == true) Ti.print("T~"); //$NON-NLS-1$

    DoubleMatrix ta = T.getSubMatrix(1, rank, 1, rank);
    DoubleMatrix tb = T.getSubMatrix(1, rank, rank + 1, T.getColumnSize());
    DoubleMatrix tc = T.getSubMatrix(rank + 1, T.getRowSize(), 1, rank);
    DoubleMatrix tA = Ti.getSubMatrix(1, rank, 1, rank);
    DoubleMatrix tB = Ti.getSubMatrix(1, rank, rank + 1, T.getColumnSize());
    DoubleMatrix tC = Ti.getSubMatrix(rank + 1, T.getRowSize(), 1, rank);

    if (debugFlag == true) {
      ta.print("a"); //$NON-NLS-1$
      tb.print("b"); //$NON-NLS-1$
      tc.print("c"); //$NON-NLS-1$
      tA.print("A"); //$NON-NLS-1$
      tB.print("B"); //$NON-NLS-1$
      tC.print("C"); //$NON-NLS-1$
      Ti.multiply(E2).multiply(T).print("T~*E*T"); //$NON-NLS-1$
      Eb.roundToZeroElementWise(DoubleNumberUtil.EPS).print("Eb"); //$NON-NLS-1$

      tA.multiply(ta).print();
      tA.multiply(tb).print();
      tC.multiply(ta).print();
      tC.multiply(tb).print();
      tA.roundToZeroElementWise(DoubleNumberUtil.EPS).print();
      Eb.roundToZeroElementWise(DoubleNumberUtil.EPS).print();
    }

    DoubleMatrix Esb = Eb.getSubMatrix(1, rank, rank + 1, Eb.getColumnSize());
    DoubleMatrix tE = Eb.createUnit();
    tE.setSubMatrix(1, rank, rank + 1, Eb.getColumnSize(), Esb.unaryMinus());

    if (debugFlag == true) {
      tE.print();
      Eb.multiply(tE).print();
      Ab.multiply(tE).print();
      Cb.multiply(tE).print();
    }

    return new ArrayList<>(Arrays.asList(new DoubleMatrix[] {numerator, denominator}));
  }

  /**
   * 与えられたベクトルの次数を適当に下げます．
   * 
   * @param fullMatrix full matrix
   * 
   * @param identifier 分子ならば"numerator",分母ならば"denominator"
   * @return　適当に次数を下げられたベクトル
   */
  private static DoubleMatrix stripLeadingZeros(final DoubleMatrix fullMatrix, final String identifier) {
    final IntMatrix idx = fullMatrix.compareElementWise(".!=", 0).find(); //$NON-NLS-1$
    if (idx.length() > 0) {
      return fullMatrix.getSubVector(idx.getIntElement(1), fullMatrix.length());
    }
    if (identifier.equals("numerator")) return fullMatrix.createOnes(1); //$NON-NLS-1$
    if (identifier.equals("denominator")) return fullMatrix.createZero(1); //$NON-NLS-1$
    throw new RuntimeException("分子ならば\"numerator\",分母ならば\"denominator\"を指定してください"); //$NON-NLS-1$
  }

  /**
   * @param a ゲイン行列
   * @param b 入力行列
   * @param c 出力行列
   * @param d ゲイン行列
   * @param e ディスクリプター行列
   * @param simplify 単純化するならばtrue
   * @return {numerator, denominator}
   */
  public static List<DoubleMatrix> des2tf(DoubleMatrix a, DoubleMatrix b, DoubleMatrix c, DoubleMatrix d, DoubleMatrix e, boolean simplify) {
    if ((d.getRowSize() != 1 || d.getColumnSize() != 1) && simplify) {
      throw new RuntimeException("現在、単純化はSISOシステムのみに対応しています．"); //$NON-NLS-1$
    }
    return des2tf(a, b, c, d, e);
  }
}