Minreal.java

/*
 * $Id: Minreal.java,v 1.25 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.matrix.ComplexNumericalMatrix;
import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.matrix.IntMatrix;
import org.mklab.nfc.matrix.RealNumericalMatrix;
import org.mklab.nfc.scalar.ComplexNumericalScalar;
import org.mklab.nfc.scalar.DoubleNumber;
import org.mklab.nfc.scalar.RealNumericalScalar;


/**
 * 最小実現を求めるクラスです。
 * 
 * <p>Minimal realization
 * 
 * @author koga
 * @version $Revision: 1.25 $
 * @see org.mklab.tool.control.Balreal
 * @see org.mklab.tool.control.Dbalreal
 */
public class Minreal {

  /**
   * システム(A,B,C,D)の最小実現を返します。
   * 
   * @param A 元のA行列
   * @param B 元のB行列
   * @param C 元のC行列
   * @param D 元のD行列
   * @return {Am, Bm, Cm, Dm} (最小実現) realization
   */
  public static List<DoubleMatrix> minreal(DoubleMatrix A, DoubleMatrix B, DoubleMatrix C, DoubleMatrix D) {
    return minreal(A, B, C, D, -1);
  }

  /**
   * システム(A,B,C,D)の最小実現を返します。
   * 
   * @param <RS> スカラーの型
   * @param <RM> 行列の型
   * @param <CS> 複素スカラーの型
   * @param <CM> 複素行列の型
   * @param A 元のA行列
   * @param B 元のB行列
   * @param C 元のC行列
   * @param D 元のD行列
   * @return {Am, Bm, Cm, Dm} (最小実現) realization
   */
  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>> List<RM> minreal(
      RM A, RM B, RM C, RM D) {
    return minreal(A, B, C, D,  A.getElement(1,1).create(-1));
  }

  /**
   * 状態の次数を決定するための許容誤差として<code>tolerance</code>を用いる。
   * 
   * @param A 元のA行列
   * @param B 元のB行列
   * @param C 元のC行列
   * @param D 元のD行列
   * @param tolerance 許容誤差
   * @return {Am, Bm, Cm, Dm} (最小実現) realization
   */
  public static List<DoubleMatrix> minreal(DoubleMatrix A, DoubleMatrix B, DoubleMatrix C, DoubleMatrix D, double tolerance) {
    return minreal(A, B, C, D, new DoubleNumber(tolerance));
  }

  /**
   * 状態の次数を決定するための許容誤差として<code>tolerance</code>を用いる。
   * 
   * @param A 元のA行列
   * @param B 元のB行列
   * @param C 元のC行列
   * @param D 元のD行列
   * @param tolerance 許容誤差
   * @return {Am, Bm, Cm, Dm} (最小実現) realization
   */
  public static List<DoubleMatrix> minreal(DoubleMatrix A, DoubleMatrix B, DoubleMatrix C, DoubleMatrix D, DoubleNumber tolerance) {
    final String message;
    if ((message = Abcdchk.abcdchk(A, B, C, D)).length() > 0) {
      throw new IllegalArgumentException(message);
    }

    final List<DoubleMatrix> abcd = localMinreal(A, B, C, D, tolerance);
    final DoubleMatrix Am = abcd.get(0);
    final DoubleMatrix Bm = abcd.get(1);
    final DoubleMatrix Cm = abcd.get(2);
    final DoubleMatrix Dm = abcd.get(3);

    return new ArrayList<>(Arrays.asList(new DoubleMatrix[] {Am, Bm, Cm, Dm}));
  }

  /**
   * 状態の次数を決定するための許容誤差として<code>tolerance</code>を用いる。
   * 
   * @param <RS> スカラーの型
   * @param <RM> 行列の型
   * @param <CS> 複素スカラーの型
   * @param <CM> 複素行列の型
   * @param A 元のA行列
   * @param B 元のB行列
   * @param C 元のC行列
   * @param D 元のD行列
   * @param tolerance 許容誤差
   * @return {Am, Bm, Cm, Dm} (最小実現) realization
   */
  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>> List<RM> minreal(
      RM A, RM B, RM C, RM D, RS tolerance) {
    final String message;
    if ((message = Abcdchk.abcdchk(A, B, C, D)).length() > 0) {
      throw new IllegalArgumentException(message);
    }

    final List<RM> abcd = localMinreal(A, B, C, D, tolerance);
//    final RM Am = abcd.get(0);
//    final RM Bm = abcd.get(1);
//    final RM Cm = abcd.get(2);
//    final RM Dm = abcd.get(3);

    return abcd;
  }

  /**
   * 次数が<code>order</code>となるよう、許容誤差を自動的に決めます。
   * 
   * @param A 元のA行列
   * @param B 元のB行列
   * @param C 元のC行列
   * @param D 元のD行列
   * @param tolerance 許容誤差
   * @param order 次数
   * @return {Am, Bm, Cm, Dm, tol} (最小実現, 許容誤差) realization
   */
  public static List<DoubleMatrix> minreal(DoubleMatrix A, DoubleMatrix B, DoubleMatrix C, DoubleMatrix D, double tolerance, int order) {
    return minreal(A, B, C, D, new DoubleNumber(tolerance), order);
  }

  /**
   * 次数が<code>order</code>となるよう、許容誤差を自動的に決めます。
   * 
   * @param A 元のA行列
   * @param B 元のB行列
   * @param C 元のC行列
   * @param D 元のD行列
   * @param tolerance 許容誤差
   * @param order 次数
   * @return {Am, Bm, Cm, Dm} (最小実現) realization
   */
  public static List<DoubleMatrix> minreal(DoubleMatrix A, DoubleMatrix B, DoubleMatrix C, DoubleMatrix D, DoubleNumber tolerance, int order) {
    final String message;
    if ((message = Abcdchk.abcdchk(A, B, C, D)).length() > 0) {
      throw new IllegalArgumentException(message);
    }

    if (order < 1 || A.getRowSize() < order) {
      throw new IllegalArgumentException("Minreal: " + Messages.getString("Minreal.0")); //$NON-NLS-1$ //$NON-NLS-2$
    }

    DoubleMatrix Am, Bm, Cm, Dm;
    DoubleNumber localTolerance = tolerance;
    int flag = 0;
    do {
      List<DoubleMatrix> abcd = localMinreal(A, B, C, D, localTolerance);
      Am = abcd.get(0);
      Bm = abcd.get(1);
      Cm = abcd.get(2);
      Dm = abcd.get(3);

      if (Am.getRowSize() == order) {
        break;
      }

      if (order < Am.getRowSize()) {
        if (flag == 2) {
          break;
        }
        localTolerance = localTolerance.multiply(2);
        flag = 1;
      } else if (order > Am.getRowSize()) {
        if (flag == 1) {
          break;
        }
        localTolerance = localTolerance.divide(2);
        flag = 2;
      }
    } while (true);

    return new ArrayList<>(Arrays.asList(new DoubleMatrix[] {Am, Bm, Cm, Dm}));
  }

  /**
   * @param A A
   * @param B B
   * @param C C
   * @param D D
   * @param tolerance 許容誤差
   * @return {Am, Bm, Cm, Dm} (最小実現) realization
   */
  private static List<DoubleMatrix> localMinreal(DoubleMatrix A, DoubleMatrix B, DoubleMatrix C, DoubleMatrix D, DoubleNumber tolerance) {
    int n = A.getRowSize(); // number of states of the given system
    int m = B.getColumnSize(); // number of inputs
    int p = C.getRowSize(); // number of outputs

    List<DoubleMatrix> abctkCtrf = Ctrf.ctrf(A, B, C, tolerance);
    DoubleMatrix Am = abctkCtrf.get(0);
    DoubleMatrix Bm = abctkCtrf.get(1);
    DoubleMatrix Cm = abctkCtrf.get(2);
    DoubleMatrix K = abctkCtrf.get(4);
    int k = (int)K.sum().doubleValue();

    if (k > 0) {
      int uc = n - k; // number of uncontrollable mode
      Am = Am.getSubMatrix(uc + 1, n, uc + 1, n);
      Bm = Bm.getRowVectors(uc + 1, n);
      Cm = Cm.getColumnVectors(uc + 1, n);
      n = n - uc; // number of controllable mode
    } else {
      Am = A.createZero(0, 0);
      Bm = A.createZero(0, m);
      Cm = A.createZero(p, 0);
    }

    if (n > 0) {
      final List<DoubleMatrix> abctkObsf = Obsf.obsf(Am, Bm, Cm, tolerance);
      Am = abctkObsf.get(0);
      Bm = abctkObsf.get(1);
      Cm = abctkObsf.get(2);
      K = abctkObsf.get(4);

      k = (int)K.sum().doubleValue();
      if (k > 0) {
        int uo = n - k; // number of unobserbale mode
        Am = Am.getSubMatrix(uo + 1, n, uo + 1, n);
        Bm = Bm.getRowVectors(uo + 1, n);
        Cm = Cm.getColumnVectors(uo + 1, n);
      } else {
        Am = A.createZero(0, 0);
        Bm = A.createZero(0, m);
        Cm = A.createZero(p, 0);
      }
    }

    DoubleMatrix Dm = D.createClone();

    return new ArrayList<>(Arrays.asList(new DoubleMatrix[] {Am, Bm, Cm, Dm}));
  }

  /**
   * @param <RS> スカラーの型
   * @param <RM> 行列の型
   * @param <CS> 複素スカラーの型
   * @param <CM> 複素行列の型
   * @param A A
   * @param B B
   * @param C C
   * @param D D
   * @param tolerance 許容誤差
   * @return {Am, Bm, Cm, Dm} (最小実現) realization
   */
  private 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>> List<RM> localMinreal(
      RM A, RM B, RM C, RM D, RS tolerance) {
    int n = A.getRowSize(); // number of states of the given system
    int m = B.getColumnSize(); // number of inputs
    int p = C.getRowSize(); // number of outputs

    CtrfSolution<RS, RM, CS, CM> abctkCtrf = Ctrf.ctrf(A, B, C, tolerance);
    RM Am = abctkCtrf.A;
    RM Bm = abctkCtrf.B;
    RM Cm = abctkCtrf.C;
    IntMatrix K = abctkCtrf.K;
    int k = K.intSum();

    if (k > 0) {
      int uc = n - k; // number of uncontrollable mode
      Am = Am.getSubMatrix(uc + 1, n, uc + 1, n);
      Bm = Bm.getRowVectors(uc + 1, n);
      Cm = Cm.getColumnVectors(uc + 1, n);
      n = n - uc; // number of controllable mode
    } else {
      Am = A.createZero(0, 0);
      Bm = A.createZero(0, m);
      Cm = A.createZero(p, 0);
    }

    if (n > 0) {
      final ObsfSolution<RS,RM,CS,CM> abctkObsf = Obsf.obsf(Am, Bm, Cm, tolerance);
      Am = abctkObsf.A;
      Bm = abctkObsf.B;
      Cm = abctkObsf.C;
      K = abctkObsf.K;

      k = K.intSum();
      if (k > 0) {
        int uo = n - k; // number of unobserbale mode
        Am = Am.getSubMatrix(uo + 1, n, uo + 1, n);
        Bm = Bm.getRowVectors(uo + 1, n);
        Cm = Cm.getColumnVectors(uo + 1, n);
      } else {
        Am = A.createZero(0, 0);
        Bm = A.createZero(0, m);
        Cm = A.createZero(p, 0);
      }
    }

    RM Dm = D.createClone();
    
    List<RM> ans = new ArrayList<>();
    ans.add(Am);
    ans.add(Bm);
    ans.add(Cm);
    ans.add(Dm);

    return ans;
    //return new ArrayList<>(Arrays.asList(new DoubleMatrix[] {Am, Bm, Cm, Dm}));
  }

}