FrequencyResponse.java

/*
 * Created on 2007/03/27
 * Copyright (C) 2007 Koga Laboratory. All rights reserved.
 *
 */
package org.mklab.tool.control;

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

import org.mklab.nfc.eig.BalancedDecomposition;
import org.mklab.nfc.eig.HessenbergDecomposition;
import org.mklab.nfc.matrix.ComplexNumericalMatrix;
import org.mklab.nfc.matrix.RealNumericalMatrix;
import org.mklab.nfc.scalar.ComplexNumericalScalar;
import org.mklab.nfc.scalar.RealNumericalScalar;


/**
 * 周波数応答を表わすクラスです。
 * 
 * @author koga
 * @version $Revision: 1.10 $, 2007/03/27
 * @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 class FrequencyResponse<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>> {

  /** システム */
  private LinearSystem<RS, RM, CS, CM> system;

  /**
   * 新しく生成された<code>FrequencyResponse</code>オブジェクトを初期化します。
   * 
   * @param system 線形システム
   */
  public FrequencyResponse(final LinearSystem<RS, RM, CS, CM> system) {
    this.system = system;
  }

  /**
   * 周波数応答を返します。
   * 
   * @param s 評価する周波数(radian/s)
   * @return 周波数応答
   */
  public List<CM> getResponse(final CM s) {
    if (this.system.isProper()) {
      return getResponseForProperSystem(s);
    }

    return getResponseForNonProperSystem(s);
  }

  /**
   * システムの周波数応答を返します。
   * 
   * @param s 評価する周波数(radian/s)
   * @param inputNumber 入力番号
   * @return 周波数応答
   */
  public CM getResponse(final CM s, final int inputNumber) {
    if (this.system.isProper()) {
      return getResponseForProperSystem(s, inputNumber);
    }

    return getResponseForNonProperSystem(s, inputNumber);
  }

  //  /**
  //   * 非プロパーなシステムの周波数応答を返します。
  //   * 
  //   * @param s 評価する周波数(radian/s)
  //   * @return 周波数応答
  //   */
  //  private List<DoubleMatrix> getResponseForNonProperSystem(final RM s) {
  //    DoubleRationalPolynomialMatrix G = this.system.getTransferFunctionMatrix();
  //
  //    final List<DoubleMatrix> responses = new ArrayList<DoubleMatrix>();
  //    for (int i = 1; i <= s.length(); i++) {
  //      final RM response = G.evaluate(s.getElement(i));
  //      responses.add(response);
  //    }
  //
  //    return responses;
  //  }
  //
  //  /**
  //   * 非プロパーなシステムの周波数応答を返します。
  //   * 
  //   * @param s 評価する周波数(radian/s)
  //   * @param inputNumber 入力番号
  //   * @return 非プロパーなシステムの周波数応答
  //   */
  //  private RM getResponseForNonProperSystem(final RM s, final int inputNumber) {
  //    final DoubleRationalPolynomialMatrix G = this.system.getTransferFunctionMatrix();
  //
  //    return (DoubleMatrix)G.getColumnVector(inputNumber).evaluate(s);
  //  }

  /**
   * 非プロパーなシステムの周波数応答を返します。
   * 
   * @param s 評価する周波数(radian/s)
   * @param inputNumber 入力番号
   * @return 非プロパーな周波数応答
   */
  private CM getResponseForNonProperSystem(final CM s, final int inputNumber) {
    final RM a = this.system.getA();
    final RM b = this.system.getB();
    final RM c = this.system.getC();
    final RM d = this.system.getD();
    final RM e = this.system.getE();

    // Balance A
    final BalancedDecomposition<RS, RM> ta = a.balancedDecompose();
    final RM t = ta.getD();
    final RM a1 = ta.getB();
    final RM b1 = t.leftDivide(b);
    final RM c1 = c.multiply(t);
    final RM e1 = t.leftDivide(e).multiply(t);

    final HessenbergDecomposition<RS, RM> qa = a1.hessenbergDecompose();
    final RM q = qa.getQ();
    final RM a2 = qa.getH();
    final RM b2 = q.conjugateTranspose().multiply(b1.getColumnVector(inputNumber));
    final RM c2 = c1.multiply(q);
    final RM d2 = d.getColumnVector(inputNumber);
    final RM e2 = q.conjugateTranspose().multiply(e1).multiply(q);

    // Frequency response
    final RM djw = d2.vectorToDiagonal().multiply(d2.createOnes(c2.getRowSize(), s.length()));

    if (a.getRowSize() == 0) {
      return s.create(djw);
    }

    final CM c_jwI_a_b = c2.toComplex().multiply(Ltifr.ltifr(a2, b2, e2, s));
    return djw.toComplex().add(c_jwI_a_b);
  }

  /**
   * 非プロパーなシステムの周波数応答を返します。
   * 
   * @param s 評価する周波数(radian/s)
   * @return 非プロパーな周波数応答
   */
  private List<CM> getResponseForNonProperSystem(final CM s) {
    final RM a = this.system.getA();
    final RM b = this.system.getB();
    final RM c = this.system.getC();
    final RM d = this.system.getD();
    final RM e = this.system.getE();

    // Balance A
    final BalancedDecomposition<RS, RM> ta = a.balancedDecompose();
    final RM t = ta.getD();
    final RM a1 = ta.getB();
    final RM b1 = t.leftDivide(b);
    final RM c1 = c.multiply(t);
    final RM e1 = t.leftDivide(e).multiply(t);

    final HessenbergDecomposition<RS, RM> qa = a1.hessenbergDecompose();
    final RM q = qa.getQ();
    final RM a2 = qa.getH();
    final RM b2 = q.conjugateTranspose().multiply(b1);
    final RM c2 = c1.multiply(q);
    final RM d2 = d;
    final RM e2 = q.conjugateTranspose().multiply(e1).multiply(q);

    // Frequency response

    final int size = s.length();

    if (a.getRowSize() == 0) {
      CM dResponse = s.create(d2);
      List<CM> responses = new ArrayList<>();
      for (int i = 0; i < size; i++) {
        responses.add(dResponse.createClone());
      }
      return responses;
    }

    List<CM> abResponses = Ltifr.getResponse(a2, b2, e2, s);
    List<CM> responses = new ArrayList<>();

    for (CM abResponse : abResponses) {
      responses.add(c2.toComplex().multiply(abResponse).add(d2.toComplex()));
    }

    return responses;
  }

  /**
   * プロパーなシステムの周波数応答を返します。
   * 
   * @param s 評価する周波数(radian/s)
   * @param inputNumber 入力番号
   * @return 周波数応答
   */
  private CM getResponseForProperSystem(final CM s, final int inputNumber) {
    final RM a = this.system.getA();
    final RM b = this.system.getB();
    final RM c = this.system.getC();
    final RM d = this.system.getD();

    // Balance A
    final BalancedDecomposition<RS, RM> ta = a.balancedDecompose();
    final RM t = ta.getD();
    final RM a1 = ta.getB();
    final RM b1 = t.leftDivide(b);
    final RM c1 = c.multiply(t);

    final HessenbergDecomposition<RS, RM> qa = a1.hessenbergDecompose();
    final RM q = qa.getQ();
    final RM a2 = qa.getH();
    final RM b2 = q.conjugateTranspose().multiply(b1.getColumnVector(inputNumber));
    final RM c2 = c1.multiply(q);
    final RM d2 = d.getColumnVector(inputNumber);

    // Frequency response
    final RM djw = d2.vectorToDiagonal().multiply(d2.createOnes(c2.getRowSize(), s.length()));

    if (a.getRowSize() == 0) {
      return s.create(djw);
    }

    final CM c_jwI_a_b = c2.toComplex().multiply(Ltifr.ltifr(a2, b2, s));
    return djw.toComplex().add(c_jwI_a_b);
  }

  /**
   * プロパーなシステムの周波数応答を返します。
   * 
   * @param s 評価する周波数(radian/s)
   * @return 周波数応答
   */
  private List<CM> getResponseForProperSystem(final CM s) {
    final RM a = this.system.getA();
    final RM b = this.system.getB();
    final RM c = this.system.getC();
    final RM d = this.system.getD();

    // Balance A
    final BalancedDecomposition<RS, RM> ta = a.balancedDecompose();
    final RM t = ta.getD();
    final RM a1 = ta.getB();
    final RM b1 = t.leftDivide(b);
    final RM c1 = c.multiply(t);

    final HessenbergDecomposition<RS, RM> qa = a1.hessenbergDecompose();
    final RM q = qa.getQ();
    final RM a2 = qa.getH();
    final RM b2 = q.conjugateTranspose().multiply(b1);
    final RM c2 = c1.multiply(q);
    final RM d2 = d;

    // Frequency response

    final int size = s.length();

    if (a.getRowSize() == 0) {
      CM dResponse = s.create(d2);
      List<CM> responses = new ArrayList<>();
      for (int i = 0; i < size; i++) {
        responses.add(dResponse.createClone());
      }
      return responses;
    }

    List<CM> abResponses = Ltifr.getResponse(a2, b2, s);
    List<CM> responses = new ArrayList<>();

    for (CM abResponse : abResponses) {
      responses.add(c2.toComplex().multiply(abResponse).add(d2.toComplex()));
    }

    return responses;
  }
}