Nyquist.java

/*
 * $Id: Nyquist.java,v 1.64 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.List;

import org.mklab.nfc.matrix.AnyRealRationalPolynomialMatrix;
import org.mklab.nfc.matrix.ComplexNumericalMatrix;
import org.mklab.nfc.matrix.IntMatrix;
import org.mklab.nfc.matrix.RealNumericalMatrix;
import org.mklab.nfc.scalar.AnyRealRationalPolynomial;
import org.mklab.nfc.scalar.ComplexNumericalScalar;
import org.mklab.nfc.scalar.RealNumericalScalar;


/**
 * 連続時間システムのナイキスト線図を描画するためのデータ(周波数応答)を求めるクラスです。
 * 
 * <p>Nyquist response of MIMO continuous-time linear state system
 * 
 * @author koga
 * @version $Revision: 1.64 $
 * @see org.mklab.tool.control.Bode
 * @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 Nyquist<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;
  /** サンプル周期 */
  private RS samplingInterval;

  /** 正の周波数について描画するならばtrue */
  private boolean isDrawingForPositiveFrequency = true;

  /** 負の周波数について描画するならばtrue */
  private boolean isDrawingForNegativeFrequency = true;

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

  /**
   * 新しく生成された<code>Nyquist</code>オブジェクトを初期化します。
   * 
   * @param system 線形システム
   * @param samplingInterval サンプリング周期
   */
  public Nyquist(final LinearSystem<RS, RM, CS, CM> system, final RS samplingInterval) {
    this.system = (LinearSystem<RS, RM, CS, CM>)system.clone();
    this.samplingInterval = samplingInterval;
    this.system.setTimeDomainType(TimeDomainType.DISCRETE);
  }

  /**
   * システムのナイキスト応答(実部、虚部、評価周波数)を返します。
   * 
   * @param angularFrequncies 評価する周波数(radian/s)
   * @return システムのナイキスト応答(実部、虚部、評価周波数)
   */
  public List<List<RM>> getRealAndImag(final RM angularFrequncies) {
    final List<List<RM>> realImagList = new ArrayList<>();
    for (int inputNumber = 1; inputNumber <= this.system.getInputSize(); inputNumber++) {
      final List<RM> realImag = getRealAndImag(angularFrequncies, inputNumber);
      realImagList.add(realImag);
    }

    return realImagList;
  }

  /**
   * システムの<code>inputNumber</code>番目の入力に関するナイキスト応答(実部、虚部、評価周波数)を返します。
   * 
   * @param angularFrequncies 評価する周波数(rad/sec)の列
   * @param inputNumber 入力番号
   * @return 周波数応答(実部、虚部、評価周波数)
   */
  public List<RM> getRealAndImag(final RM angularFrequncies, final int inputNumber) {
    final CM poles;

    if (this.system.isProper()) {
      poles = this.system.getA().eigenValue();
    } else {
      final AnyRealRationalPolynomialMatrix<RS, RM, CS, CM> denominators = this.system.getTransferFunctionMatrix();
      final AnyRealRationalPolynomial<RS, RM, CS, CM>[][] pp = denominators.getElement(1, 1).createArray(this.system.getOutputSize(), this.system.getInputSize());
      for (int i = 0; i < this.system.getOutputSize(); i++) {
        for (int j = 0; j < this.system.getInputSize(); j++) {
          pp[i][j] = denominators.getElement(i, j).inverse();
        }
      }

      final AnyRealRationalPolynomialMatrix<RS, RM, CS, CM> G2 = new AnyRealRationalPolynomialMatrix<>(pp);
      poles = LinearSystemFactory.createLinearSystem(G2).getA().eigenValue();
    }

    final RM negativeFrequency = angularFrequncies.flipLeftRight().unaryMinus();
    final RM positiveFrequency = angularFrequncies;

    RM ww = angularFrequncies.createZero(angularFrequncies.getRowSize(), 0);
    if (isDrawingForNegativeFrequency()) {
      ww = negativeFrequency;
    }
    if (isDrawingForPositiveFrequency()) {
      ww = ww.appendRight(positiveFrequency);
    }

    RS sunit = ww.getElement(1, 1).createUnit();
    final CS j = sunit.toComplex().create(sunit.createZero(), sunit.createUnit());
    final CM jw = ww.toComplex().multiply(j);

    IntMatrix imaginaryAxisPolesIndices = poles.getRealPart().compareElementWise(".==", 0).find(); //$NON-NLS-1$

    if (imaginaryAxisPolesIndices.length() != 0) {
      final CM imaginaryAxisPoles = poles.getSubVector(imaginaryAxisPolesIndices);
      final RS radius = sunit.create(10).inverse();

      for (int i = 1; i <= jw.length(); i++) {
        final RM distanceFromPoles = (imaginaryAxisPoles.subtractElementWise(jw.getElement(i))).absElementWise().getRealPart();
        final RS distance = distanceFromPoles.min();
        if (distance.isLessThan(radius)) {
          final RS shift = distance.power(2).unaryMinus().add(radius.multiply(radius)).sqrt();
          jw.setElement(i, jw.getElement(i).add(shift.toComplex()));
        }
      }
    }

    final CM gjw = new FrequencyResponse<>(this.system).getResponse(jw, inputNumber);

    List<RM> ri = new ArrayList<>();
    ri.add(gjw.getRealPart());
    ri.add(gjw.getImaginaryPart());
    ri.add(angularFrequncies);

    return ri;
  }

  //  /**
  //   * メインメソッド
  //   * 
  //   * @param args コマンドライン引数
  //   * @throws IOException キーボードから入力できない場合
  //   */
  //  public static void main(String[] args) throws IOException {
  //    final DoublePolynomial s = new DoublePolynomial("s"); //$NON-NLS-1$
  //    final DoubleRationalPolynomial g1 = new DoubleRationalPolynomial(new DoublePolynomial(1), s.add(1));
  //    final DoubleRationalPolynomial g2 = new DoubleRationalPolynomial(new DoublePolynomial(10), s.add(10));
  //    final DoubleRationalPolynomial g = g1.multiply(g1).multiply(g2);
  //
  //    final Gnuplot gnuplot = new Gnuplot();
  //    new Nyquist<>(LinearSystemFactory.createLinearSystem(g)).plot(gnuplot);
  //
  //    try {
  //      Pause.pause();
  //    } finally {
  //      gnuplot.close();
  //    }
  //  }

  /**
   * 正の周波数について描画するか判定します。
   * 
   * @return 正の周波数について描画するならばtrue
   */
  public boolean isDrawingForPositiveFrequency() {
    return this.isDrawingForPositiveFrequency;
  }

  /**
   * 正の周波数について描画するか設定します。
   * 
   * @param isDrawingForPositiveFrequency 正の周波数について描画するならばtrue
   */
  public void setDrawingForPositiveFrequency(boolean isDrawingForPositiveFrequency) {
    this.isDrawingForPositiveFrequency = isDrawingForPositiveFrequency;
  }

  /**
   * 負の周波数について描画するか判定します。
   * 
   * @return 負の周波数について描画するならばtrue
   */
  public boolean isDrawingForNegativeFrequency() {
    return this.isDrawingForNegativeFrequency;
  }

  /**
   * 負の周波数について描画するか設定します。
   * 
   * @param isDrawingForNegativeFrequency 負の周波数について描画するならばtrue
   */
  public void setDrawingForNegativeFrequency(boolean isDrawingForNegativeFrequency) {
    this.isDrawingForNegativeFrequency = isDrawingForNegativeFrequency;
  }
}