Ctrf.java

/*
 * $Id: Ctrf.java,v 1.33 2008/05/13 04:26:12 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.matrix.ComplexNumericalMatrix;
import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.matrix.IntMatrix;
import org.mklab.nfc.matrix.NormType;
import org.mklab.nfc.matrix.RealNumericalMatrix;
import org.mklab.nfc.scalar.ComplexNumericalScalar;
import org.mklab.nfc.scalar.DoubleNumber;
import org.mklab.nfc.scalar.RealNumericalScalar;
import org.mklab.nfc.svd.SingularValueDecomposition;
import org.mklab.tool.matrix.Diag;


/**
 * 可制御部分と不可制御部分に分解するクラスです。
 * 
 * <p>Controllable-uncontrollable decomposition
 * 
 * @author koga
 * @version $Revision: 1.33 $
 * @see org.mklab.tool.control.Ctrm
 * @see org.mklab.tool.control.Obsf
 */
public class Ctrf {

  /**
   * 可制御部分と不可制御部分に分解したシステム表現を返します。
   * 
   * <p> もし <code>rank([A B]) &lt; Rows(A)</code>なら、相似変換 <code>z = T x</code>により、システム行列を
   * 
   * <pre><code> Ab = T*A*T# , Bb = T*B , Cb = C*T# </code></pre>
   * 
   * のように変換すると、それぞれの行列は
   * 
   * <pre><code> [Anc| 0] Ab = [---+--], [A21|Ac]
   * 
   * [ 0] Bb = [--], [Bc]
   * 
   * Cb = [Cnc|Cc].
   * 
   * </code></pre>
   * 
   * のようになる。ただし、<code>(Ac,Bc)</code>は可制御であり、
   * 
   * <pre><code>
   * 
   * -1 -1 Cc(sI-Ac)Bc = C(sI-A)B. </code></pre>
   * 
   * が成り立ちます。
   * 
   * @param A 元のA行列
   * @param B 元のB行列
   * @param C 元のD行列
   * @return { Ab, Bb, Cb, T, k } (controllable-uncontrollable decomposition)
   */
  public static List<DoubleMatrix> ctrf(DoubleMatrix A, DoubleMatrix B, DoubleMatrix C) {
    int stateSize = A.getRowSize();
    DoubleNumber unit = A.getElement(1, 1);

    DoubleNumber tolerance = A.norm(NormType.ONE).multiply(stateSize).multiply(unit.getMachineEpsilon());
    return ctrf(A, B, C, tolerance);
  }

  /**
   * 部分空間の次元を決めるために許容誤差<code>tolerance</code>を用いる。
   * 
   * @param A 元のA行列
   * @param B 元のB行列
   * @param C 元のD行列
   * @param tolerance 許容誤差
   * @return { Ab, Bb, Cb, T, k } (controllable-uncontrollable decomposition)
   */
  public static List<DoubleMatrix> ctrf(DoubleMatrix A, DoubleMatrix B, DoubleMatrix C, double tolerance) {
    return ctrf(A, B, C, new DoubleNumber(tolerance));
  }

//  /**
//   * 部分空間の次元を決めるために許容誤差<code>tolerance</code>を用いる。
//   * 
//   * @param <RS> スカラーの型
//   * @param <RM> 行列の型
//   * @param <CS> 複素スカラーの型
//   * @param <CM> 複素行列の型
//   * @param A 元のA行列
//   * @param B 元のB行列
//   * @param C 元のD行列
//   * @param tolerance 許容誤差
//   * @return { Ab, Bb, Cb, T, k } (controllable-uncontrollable decomposition)
//   */
//  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>> CtrfSolution<RS, RM, CS, CM> ctrf(
//      RM A, RM B, RM C, RS tolerance) {
//    return ctrf(A, B, C, A.getElement(1, 1).create(tolerance));
//  }

  /**
   * 部分空間の次元を決めるために許容誤差<code>tolerance</code>を用いる。
   * 
   * @param A 元のA行列
   * @param B 元のB行列
   * @param C 元のD行列
   * @param tolerance 許容誤差
   * @return { Ab, Bb, Cb, T, k } (controllable-uncontrollable decomposition)
   */
  public static List<DoubleMatrix> ctrf(DoubleMatrix A, DoubleMatrix B, DoubleMatrix C, DoubleNumber tolerance) {
    String message;
    if ((message = Abcdchk.abcdchk(A, B, C)).length() > 0) {
      throw new IllegalArgumentException(message);
    }

    int stateSize = A.getRowSize();
    IntMatrix k = new IntMatrix(1, stateSize);

    final DoubleNumber localTolerance;
    if (tolerance.isLessThan(0)) {
      DoubleNumber aNorm = A.norm(NormType.ONE);
      aNorm = aNorm.isGreaterThan(1.0E5) ? aNorm.createUnit().multiply(1.0E5) : aNorm;
      localTolerance = (aNorm.multiply(aNorm.getMachineEpsilon()));
    } else {
      localTolerance = tolerance;
    }

    int d = 0;
    DoubleMatrix P = A.createUnit(stateSize);
    DoubleMatrix An = A;
    DoubleMatrix Bn = B;
    for (int i = 1; i <= stateSize; i++) {
      SingularValueDecomposition<DoubleNumber, DoubleMatrix> udv = Bn.singularValueDecompose();
      DoubleMatrix U = udv.getU();
      DoubleMatrix D = udv.getD();

      U = U.multiply(A.createUnit(D.getRowSize()).flipLeftRight());
      DoubleMatrix BB = U.conjugateTranspose().multiply(Bn);

      final int r = BB.rank(localTolerance);

      k.setElement(1, i, r);
      final int s = BB.getRowSize() - r;

      if (r == 0 || s == 0) {
        break;
      }

      DoubleMatrix Au = U.conjugateTranspose().multiply(An).multiply(U);
      An = Au.getSubMatrix(1, s, 1, s);
      Bn = Au.getSubMatrix(1, s, s + 1, s + r);
      P = P.multiply(Diag.diag(U, A.createUnit(d)));
      d = d + r;
    }

    DoubleMatrix T = P.conjugateTranspose();
    DoubleMatrix Ab = T.multiply(A).multiply(T.conjugateTranspose());
    DoubleMatrix Bb = T.multiply(B);
    DoubleMatrix Cb = C.multiply(T.conjugateTranspose());

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

  /**
   * 部分空間の次元を決めるために許容誤差<code>tolerance</code>を用いる。
   * 
   * @param <RS> スカラーの型
   * @param <RM> 行列の型
   * @param <CS> 複素スカラーの型
   * @param <CM> 複素行列の型
   * @param A 元のA行列
   * @param B 元のB行列
   * @param C 元のD行列
   * @param tolerance 許容誤差
   * @return { Ab, Bb, Cb, T, k } (controllable-uncontrollable decomposition)
   */
  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>> CtrfSolution<RS, RM, CS, CM> ctrf(
      RM A, RM B, RM C, RS tolerance) {
    String message;
    if ((message = Abcdchk.abcdchk(A, B, C)).length() > 0) {
      throw new IllegalArgumentException(message);
    }

    int stateSize = A.getRowSize();
    IntMatrix k = new IntMatrix(1, stateSize);

    final RS localTolerance;
    if (tolerance.isLessThan(0)) {
      RS aNorm = A.norm(NormType.ONE);
      aNorm = aNorm.isGreaterThan(100000) ? aNorm.createUnit().multiply(100000) : aNorm;
      localTolerance = (aNorm.multiply(aNorm.getMachineEpsilon()));
    } else {
      localTolerance = tolerance;
    }

    int d = 0;
    RM P = A.createUnit(stateSize);
    RM An = A;
    RM Bn = B;
    for (int i = 1; i <= stateSize; i++) {
      SingularValueDecomposition<RS, RM> udv = Bn.singularValueDecompose();
      RM U = udv.getU();
      RM D = udv.getD();

      U = U.multiply(A.createUnit(D.getRowSize()).flipLeftRight());
      RM BB = U.conjugateTranspose().multiply(Bn);

      final int r = BB.rank(localTolerance);

      k.setElement(1, i, r);
      final int s = BB.getRowSize() - r;

      if (r == 0 || s == 0) {
        break;
      }

      RM Au = U.conjugateTranspose().multiply(An).multiply(U);
      An = Au.getSubMatrix(1, s, 1, s);
      Bn = Au.getSubMatrix(1, s, s + 1, s + r);
      P = P.multiply(Diag.diag(U, A.createUnit(d)));
      d = d + r;
    }

    RM T = P.conjugateTranspose();
    RM Ab = T.multiply(A).multiply(T.conjugateTranspose());
    RM Bb = T.multiply(B);
    RM Cb = C.multiply(T.conjugateTranspose());

    return new CtrfSolution<>(Ab, Bb, Cb, T, k);

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