DoubleFrequencyResponse.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.DoubleComplexMatrix;
import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.scalar.DoubleNumber;


/**
 * 周波数応答を表わすクラスです。
 * 
 * @author koga
 * @version $Revision: 1.10 $, 2007/03/27
 */
public class DoubleFrequencyResponse {

  /** システム */
  private DoubleLinearSystem system;

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

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

    return getResponseForNonProperSystem(s);
  }

  /**
   * システムの周波数応答を返します。
   * 
   * @param s 評価する周波数(radian/s)
   * @param inputNumber 入力番号
   * @return 周波数応答
   */
  public DoubleComplexMatrix getResponse(final DoubleComplexMatrix 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 DoubleMatrix s) {
  //    DoubleRationalPolynomialMatrix G = this.system.getTransferFunctionMatrix();
  //
  //    final List<DoubleMatrix> responses = new ArrayList<DoubleMatrix>();
  //    for (int i = 1; i <= s.length(); i++) {
  //      final DoubleMatrix response = G.evaluate(s.getElement(i));
  //      responses.add(response);
  //    }
  //
  //    return responses;
  //  }
  //
  //  /**
  //   * 非プロパーなシステムの周波数応答を返します。
  //   * 
  //   * @param s 評価する周波数(radian/s)
  //   * @param inputNumber 入力番号
  //   * @return 非プロパーなシステムの周波数応答
  //   */
  //  private DoubleMatrix getResponseForNonProperSystem(final DoubleMatrix 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 DoubleComplexMatrix getResponseForNonProperSystem(final DoubleComplexMatrix s, final int inputNumber) {
    final DoubleMatrix a = this.system.getA();
    final DoubleMatrix b = this.system.getB();
    final DoubleMatrix c = this.system.getC();
    final DoubleMatrix d = this.system.getD();
    final DoubleMatrix e = this.system.getE();

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

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

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

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

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

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

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

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

    // Frequency response

    final int size = s.length();

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

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

    for (DoubleComplexMatrix abResponse : abResponses) {
      responses.add(new DoubleComplexMatrix(c2).multiply(abResponse).add(new DoubleComplexMatrix(d2)));
    }

    return responses;
  }

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

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

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

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

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

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

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

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

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

    // Frequency response

    final int size = s.length();

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

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

    for (DoubleComplexMatrix abResponse : abResponses) {
      responses.add(new DoubleComplexMatrix(c2).multiply(abResponse).add(new DoubleComplexMatrix(d2)));
    }

    return responses;
  }
}