Ltifr.java

/*
 * $Id: Ltifr.java,v 1.25 2008/05/15 01:48:05 koga Exp $
 *
 * Copyright (C) 2004 Koga Laboratory. All rights reserved.
 */
package org.mklab.tool.control;

import java.util.ArrayList;
import java.util.List;

import org.mklab.nfc.matrix.ComplexNumericalMatrix;
import org.mklab.nfc.matrix.DoubleComplexMatrix;
import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.matrix.RealNumericalMatrix;
import org.mklab.nfc.scalar.ComplexNumericalScalar;
import org.mklab.nfc.scalar.DoubleComplexNumber;
import org.mklab.nfc.scalar.DoubleNumber;
import org.mklab.nfc.scalar.RealNumericalScalar;


/**
 * 連続時間線形システムの周波数応答を求めるクラスです。
 * 
 * <p>Frequency response of LTI system
 * 
 * @author koga
 * @version $Revision: 1.25 $
 * @see org.mklab.tool.control.Ltitr
 */
public class Ltifr {

  /**
   * 連続時間線形時不変システムの周波数応答
   * 
   * <pre><code> (s*I - A) \ B </code></pre>
   * 
   * を計算します。
   * 
   * @param A A行列
   * @param B B行列
   * @param s_ 評価する周波数(複素数)の列
   * @return 周波数応答 response
   */
  public static DoubleComplexMatrix ltifr(final DoubleMatrix A, final DoubleMatrix B, final DoubleComplexMatrix s_) {
    String message;
    if ((message = Abcdchk.abcdchk(A, B)).length() > 0) {
      throw new IllegalArgumentException(message);
    }

    DoubleComplexMatrix ss;
    if (s_.getRowSize() > s_.getColumnSize()) {
      ss = s_.transpose();
    } else {
      ss = s_.createClone();
    }

    final int stateSize = A.getRowSize();
    final int inputSize = B.getColumnSize();

    DoubleNumber unit = A.getElement(1, 1);
    final DoubleComplexMatrix cA = new DoubleComplexMatrix(A);

    final DoubleComplexMatrix G = cA.createZero(stateSize * ss.getRowSize(), inputSize * ss.getColumnSize());
    final int size = Math.max(ss.getRowSize(), ss.getColumnSize()); // length(s)
    final DoubleNumber epsilon = unit.getMachineEpsilon();

    for (int i = 1; i <= size; i++) {
      final DoubleComplexNumber s = ss.getElement(i);
      DoubleComplexMatrix sI_A = (new DoubleComplexMatrix(A.createUnit()).multiply(s).subtract(new DoubleComplexMatrix(A)));
      if (sI_A.isFullRank() == false) {
        sI_A = (new DoubleComplexMatrix(A.createUnit()).multiply(s.add(new DoubleComplexNumber(epsilon))).subtract(new DoubleComplexMatrix(A)));
      }

      G.setColumnVectors((i - 1) * inputSize + 1, i * inputSize, sI_A.leftDivide(new DoubleComplexMatrix(B)));
    }
    return G;
  }

  /**
   * 連続時間線形時不変システムの周波数応答
   * 
   * <pre><code> (s*E - A) \ B </code></pre>
   * 
   * を計算します。
   * 
   * @param A A行列
   * @param B B行列
   * @param E E行列
   * @param s_ 評価する周波数(複素数)の列
   * @return 非プロパーな周波数応答 response
   */
  public static DoubleComplexMatrix ltifr(final DoubleMatrix A, final DoubleMatrix B, final DoubleMatrix E, final DoubleComplexMatrix s_) {
    String message;
    if ((message = Abcdchk.abcdchk(A, B)).length() > 0) {
      throw new IllegalArgumentException(message);
    }

    DoubleComplexMatrix ss;
    if (s_.getRowSize() > s_.getColumnSize()) {
      ss = s_.transpose();
    } else {
      ss = s_.createClone();
    }

    final int stateSize = A.getRowSize();
    final int inputSize = B.getColumnSize();

    DoubleNumber unit = A.getElement(1, 1);
    final DoubleComplexMatrix cA = new DoubleComplexMatrix(A);

    final DoubleComplexMatrix G = cA.createZero(stateSize * ss.getRowSize(), inputSize * ss.getColumnSize());
    final int size = Math.max(ss.getRowSize(), ss.getColumnSize()); // length(s)
    final DoubleNumber epsilon = unit.getMachineEpsilon();

    for (int i = 1; i <= size; i++) {
      final DoubleComplexNumber s = ss.getElement(i);
      DoubleComplexMatrix sE_A = (new DoubleComplexMatrix(E).multiply(s).subtract(new DoubleComplexMatrix(A)));
      if (sE_A.isFullRank() == false) {
        sE_A = (new DoubleComplexMatrix(E).multiply(s.add(new DoubleComplexNumber(epsilon))).subtract(new DoubleComplexMatrix(A)));
      }

      G.setColumnVectors((i - 1) * inputSize + 1, i * inputSize, sE_A.leftDivide(new DoubleComplexMatrix(B)));
    }
    return G;
  }

  /**
   * 連続時間線形時不変システムの周波数応答
   * 
   * <pre><code> (s*E - A) \ B </code></pre>
   * 
   * を計算します。
   * 
   * @param A A行列
   * @param B B行列
   * @param E E行列
   * @param s_ 評価する周波数(複素数)の列
   * @return 非プロパーな周波数応答 response
   */
  public static List<DoubleComplexMatrix> getResponse(DoubleMatrix A, DoubleMatrix B, DoubleMatrix E, DoubleComplexMatrix s_) {
    String message;
    if ((message = Abcdchk.abcdchk(A, B)).length() > 0) {
      throw new IllegalArgumentException(message);
    }

    DoubleComplexMatrix ss;
    if (s_.getRowSize() > s_.getColumnSize()) {
      ss = s_.transpose();
    } else {
      ss = s_.createClone();
    }

    DoubleNumber unit = A.getElement(1, 1);

    final int size = Math.max(ss.getRowSize(), ss.getColumnSize()); // length(s)
    final DoubleNumber epsilon = unit.getMachineEpsilon();

    final List<DoubleComplexMatrix> G = new ArrayList<>();

    for (int i = 1; i <= size; i++) {
      final DoubleComplexNumber s = ss.getElement(i);
      DoubleComplexMatrix sE_A = (new DoubleComplexMatrix(E).multiply(s).subtract(new DoubleComplexMatrix(A)));
      if (sE_A.isFullRank() == false) {
        sE_A = (new DoubleComplexMatrix(E).multiply(s.add(new DoubleComplexNumber(epsilon))).subtract(new DoubleComplexMatrix(A)));
      }

      G.add(sE_A.leftDivide(new DoubleComplexMatrix(B)));
    }
    return G;
  }

  /**
   * 連続時間線形時不変システムの周波数応答
   * 
   * <pre><code> (s*I - A) \ B </code></pre>
   * 
   * を計算します。
   * 
   * @param A A行列
   * @param B B行列
   * @param s_ 評価する周波数(複素数)の列
   * @return 周波数応答 response
   * @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 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>> CM ltifr(
      final RM A, final RM B, final CM s_) {
    String message;
    if ((message = Abcdchk.abcdchk(A, B)).length() > 0) {
      throw new IllegalArgumentException(message);
    }

    CM ss;
    if (s_.getRowSize() > s_.getColumnSize()) {
      ss = s_.transpose();
    } else {
      ss = s_.createClone();
    }

    final int stateSize = A.getRowSize();
    final int inputSize = B.getColumnSize();

    RS unit = A.getElement(1, 1);
    final CM cA = A.toComplex();

    final CM G = cA.createZero(stateSize * ss.getRowSize(), inputSize * ss.getColumnSize());
    final int size = Math.max(ss.getRowSize(), ss.getColumnSize()); // length(s)
    final RS epsilon = unit.getMachineEpsilon();

    for (int i = 1; i <= size; i++) {
      final CS s = ss.getElement(i);
      CM sI_A = (A.createUnit().toComplex().multiply(s).subtract(A.toComplex()));
      if (sI_A.isFullRank() == false) {
        sI_A = (A.createUnit().toComplex().multiply(s.add(epsilon.toComplex())).subtract(A.toComplex()));
      }

      G.setColumnVectors((i - 1) * inputSize + 1, i * inputSize, sI_A.leftDivide(B.toComplex()));
    }
    return G;
  }

  /**
   * 連続時間線形時不変システムの周波数応答
   * 
   * <pre><code> (s*E - A) \ B </code></pre>
   * 
   * を計算します。
   * 
   * @param A A行列
   * @param B B行列
   * @param E E行列
   * @param s_ 評価する周波数(複素数)の列
   * @return 非プロパーな周波数応答 response
   * @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 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>> CM ltifr(
      final RM A, final RM B, final RM E, final CM s_) {
    String message;
    if ((message = Abcdchk.abcdchk(A, B)).length() > 0) {
      throw new IllegalArgumentException(message);
    }

    CM ss;
    if (s_.getRowSize() > s_.getColumnSize()) {
      ss = s_.transpose();
    } else {
      ss = s_.createClone();
    }

    final int stateSize = A.getRowSize();
    final int inputSize = B.getColumnSize();

    RS unit = A.getElement(1, 1);
    final CM cA = A.toComplex();

    final CM G = cA.createZero(stateSize * ss.getRowSize(), inputSize * ss.getColumnSize());
    final int size = Math.max(ss.getRowSize(), ss.getColumnSize()); // length(s)
    final RS epsilon = unit.getMachineEpsilon();

    for (int i = 1; i <= size; i++) {
      final CS s = ss.getElement(i);
      CM sE_A = (E.toComplex().multiply(s).subtract(A.toComplex()));
      if (sE_A.isFullRank() == false) {
        sE_A = (E.toComplex().multiply(s.add(epsilon.toComplex())).subtract(A.toComplex()));
      }

      G.setColumnVectors((i - 1) * inputSize + 1, i * inputSize, sE_A.leftDivide(B.toComplex()));
    }
    return G;
  }

  /**
   * 連続時間線形時不変システムの周波数応答
   * 
   * <pre><code> (s*E - A) \ B </code></pre>
   * 
   * を計算します。
   * 
   * @param A A行列
   * @param B B行列
   * @param E E行列
   * @param s_ 評価する周波数(複素数)の列
   * @return 非プロパーな周波数応答 response
   * @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 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<CM> getResponse(
      RM A, RM B, RM E, CM s_) {
    String message;
    if ((message = Abcdchk.abcdchk(A, B)).length() > 0) {
      throw new IllegalArgumentException(message);
    }

    CM ss;
    if (s_.getRowSize() > s_.getColumnSize()) {
      ss = s_.transpose();
    } else {
      ss = s_.createClone();
    }

    RS unit = A.getElement(1, 1);

    final int size = Math.max(ss.getRowSize(), ss.getColumnSize()); // length(s)
    final RS epsilon = unit.getMachineEpsilon();

    final List<CM> G = new ArrayList<>();

    for (int i = 1; i <= size; i++) {
      final CS s = ss.getElement(i);
      CM sE_A = (E.toComplex().multiply(s).subtract(A.toComplex()));
      if (sE_A.isFullRank() == false) {
        sE_A = (E.toComplex().multiply(s.add(epsilon.toComplex())).subtract(A.toComplex()));
      }

      G.add(sE_A.leftDivide(B.toComplex()));
    }
    return G;
  }

  /**
   * 連続時間線形時不変システムの周波数応答
   * 
   * <pre><code> (s*I - A) \ B </code></pre>
   * 
   * を計算します。
   * 
   * @param A A行列
   * @param B B行列
   * @param s_ 評価する周波数(複素数)の列
   * @return 周波数応答 response
   * @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 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<CM> getResponse(
      final RM A, final RM B, final CM s_) {
    String message;
    if ((message = Abcdchk.abcdchk(A, B)).length() > 0) {
      throw new IllegalArgumentException(message);
    }

    CM ss;
    if (s_.getRowSize() > s_.getColumnSize()) {
      ss = s_.transpose();
    } else {
      ss = s_.createClone();
    }

    RS unit = A.getElement(1, 1);

    final int size = Math.max(ss.getRowSize(), ss.getColumnSize()); // length(s)
    final RS epsilon = unit.getMachineEpsilon();

    final List<CM> G = new ArrayList<>();

    for (int i = 1; i <= size; i++) {
      final CS s = ss.getElement(i);
      CM sI_A = (A.createUnit().toComplex().multiply(s).subtract(A.toComplex()));
      if (sI_A.isFullRank() == false) {
        sI_A = (A.createUnit().toComplex().multiply(s.add(epsilon.toComplex())).subtract(A.toComplex()));
      }

      G.add(sI_A.leftDivide(B.toComplex()));
    }
    return G;
  }

}