DoubleNyquist.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.io.IOException;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;

import org.mklab.nfc.matrix.DoubleComplexMatrix;
import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.matrix.DoublePolynomialMatrix;
import org.mklab.nfc.matrix.DoubleRationalPolynomialMatrix;
import org.mklab.nfc.matrix.IntMatrix;
import org.mklab.nfc.matrix.misc.LogarithmicallySpacedVector;
import org.mklab.nfc.scalar.DoubleComplexNumber;
import org.mklab.nfc.scalar.DoubleNumber;
import org.mklab.nfc.scalar.DoublePolynomial;
import org.mklab.nfc.scalar.DoubleRationalPolynomial;
import org.mklab.nfc.util.Pause;
import org.mklab.tool.graph.gnuplot.Canvas;
import org.mklab.tool.graph.gnuplot.Gnuplot;


/**
 * 連続時間システムのナイキスト線図を描画するためのデータ(周波数応答)を求めるクラスです。
 * 
 * <p>Nyquist response of MIMO continuous-time linear state system
 * 
 * @author koga
 * @version $Revision: 1.64 $
 * @see org.mklab.tool.control.Bode
 */
public class DoubleNyquist {

  /** システム */
  private DoubleLinearSystem system;
  /** サンプル周期 */
  private double samplingInterval;

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

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

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

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

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

    return realImagList;
  }

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

    if (this.system.isProper()) {
      poles = this.system.getA().eigenValue();
    } else {
      final DoublePolynomialMatrix denominators = this.system.getTransferFunctionMatrix().getDenominatorElementWise();
      final DoubleRationalPolynomialMatrix G2 = new DoubleRationalPolynomialMatrix(denominators.createOnes(), denominators);
      poles = DoubleLinearSystemFactory.createLinearSystem(G2).getA().eigenValue();
    }

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

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

    final DoubleComplexNumber j = new DoubleComplexNumber(0,1);
    final DoubleComplexMatrix jw = new DoubleComplexMatrix(ww).multiply(j);

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

    if (imaginaryAxisPolesIndices.length() != 0) {
      final DoubleComplexMatrix imaginaryAxisPoles = poles.getSubVector(imaginaryAxisPolesIndices);
      final double radius = 1.0E-1;

      for (int i = 1; i <= jw.length(); i++) {
        final DoubleMatrix distanceFromPoles = (imaginaryAxisPoles.subtractElementWise(jw.getElement(i))).absElementWise().getRealPart();
        final DoubleNumber distance = distanceFromPoles.min();
        if (distance.isLessThan(radius)) {
          final DoubleNumber shift = distance.power(2).unaryMinus().add(radius * radius).sqrt();
          jw.setElement(i, jw.getElement(i).add(new DoubleComplexNumber(shift)));
        }
      }
    }

    final DoubleComplexMatrix gjw = new DoubleFrequencyResponse(this.system).getResponse(jw, inputNumber);

    return new ArrayList<>(Arrays.asList(new DoubleMatrix[] {gjw.getRealPart(), gjw.getImaginaryPart(), angularFrequncies}));
  }

  /**
   * ナイキスト線図をプロットします。
   * 
   * @param gnuplot Gnuplot
   */
  public void plot(final Gnuplot gnuplot) {
    final DoubleMatrix angularFrequencies;
    if (this.system.isContinuous()) {
      angularFrequencies = LogarithmicallySpacedVector.create(-2, 3, 100);
    } else {
      final double period = 2 * Math.PI / this.samplingInterval;
      angularFrequencies = LogarithmicallySpacedVector.create(-1, Math.log10(period));
    }
    plot(gnuplot, angularFrequencies);
  }

  /**
   * ナイキスト線図をプロットします。
   * 
   * @param gnuplot Gnuplot
   * @param angularFrequncies 評価する周波数(radian/s)
   */
  public void plot(Gnuplot gnuplot, DoubleMatrix angularFrequncies) {
    double reMin = Double.POSITIVE_INFINITY;
    double reMax = Double.NEGATIVE_INFINITY;
    double imMin = Double.POSITIVE_INFINITY;
    double imMax = Double.NEGATIVE_INFINITY;

    DoubleMatrix reals = new DoubleMatrix(0, angularFrequncies.length() * 2);
    DoubleMatrix imags = new DoubleMatrix(0, angularFrequncies.length() * 2);

    final List<String> names = new ArrayList<>();
    final List<String> lineAttributes = new ArrayList<>();

    int inputNumber = 1;
    int lineAttribute = 3;

    for (List<DoubleMatrix> realImag : getRealAndImag(angularFrequncies)) {
      final DoubleMatrix real = realImag.get(0);
      final DoubleMatrix imag = realImag.get(1);

      for (int outputNumber = 1; outputNumber <= real.getRowSize(); outputNumber++) {
        final DoubleMatrix re = real.getRowVector(outputNumber);
        final DoubleMatrix im = imag.getRowVector(outputNumber);

        if (re.isZero() && im.isZero()) {
          continue;
        }

        reals = reals.appendDown(re);
        imags = imags.appendDown(im);
        names.add("G(" + inputNumber + "," + outputNumber + ")"); //$NON-NLS-1$ //$NON-NLS-2$ //$NON-NLS-3$
        lineAttributes.add("with lines " + lineAttribute + " 0"); //$NON-NLS-1$ //$NON-NLS-2$
        lineAttribute++;

        reMin = Math.min(reMin, re.min().doubleValue());
        reMax = Math.max(reMax, re.max().doubleValue());
        imMin = Math.min(imMin, im.min().doubleValue());
        imMax = Math.max(imMax, im.max().doubleValue());
      }

      inputNumber++;
    }

    final Canvas canvas = gnuplot.createCanvas();
    canvas.setXLabel("Re"); //$NON-NLS-1$
    canvas.setYLabel("Im"); //$NON-NLS-1$
    canvas.setGridVisible(true);
    canvas.setTitle("Nyquist Plot"); //$NON-NLS-1$

    canvas.plot(reals, imags, names.toArray(new String[names.size()]), new String[0], lineAttributes.toArray(new String[names.size()]));

    drawMinusOneZeroPoint(canvas, reMin, reMax);
    drawRealAxis(canvas, reMin, reMax);
    drawImaginaryAxis(canvas, imMin, imMax);
  }

  /**
   * (-1,0)の点を描画します。
   * 
   * @param canvas キャンバス
   * @param reMin 実部データの最小値
   * @param reMax 実部データの最大値
   */
  private void drawMinusOneZeroPoint(final Canvas canvas, final double reMin, final double reMax) {
    if (0.1 < Math.max(Math.abs(reMin), Math.abs(reMax))) {
      canvas.setHolding(true);
      canvas.plot(new DoubleMatrix(new double[] {-1, -1}), new DoubleMatrix(1, 2), new String[] {""}, new String[0], new String[] {"with points 2 7"}); //$NON-NLS-1$ //$NON-NLS-2$
      canvas.setHolding(false);
    }
  }

  /**
   * 実軸を描画します。
   * 
   * @param canvas キャンバス
   * @param min 実部データの最小値
   * @param max 実部データの最大値
   */
  private void drawRealAxis(final Canvas canvas, final double min, final double max) {
    double reMin = min;
    double reMax = max;

    if (0.1 < Math.abs(reMax) || 0.1 < Math.abs(reMin)) {
      if (reMax < 1) {
        reMax = 1;
      } else if (100 < reMax) {
        reMax = 100;
      } else {
        reMax = Math.ceil(reMax / 10) * 10;
      }
      if (-1 < reMin) {
        reMin = -2;
      } else if (reMin < -100) {
        reMin = -100;
      } else {
        reMin = Math.floor(reMin / 10) * 10;
      }
    }

    // Real axis
    canvas.setHolding(true);
    canvas.setXRange(reMin, reMax);
    final double grid = Math.pow(10, Math.floor(Math.log10((reMax - reMin) / 10))) * 2;
    canvas.setXTics(reMin, grid, reMax);
    canvas.plot(new DoubleMatrix(new double[] {reMin, reMax}), new DoubleMatrix(1, 2), new String[] {""}, new String[0], new String[] {"with lines 1 0"}); //$NON-NLS-1$ //$NON-NLS-2$
    canvas.setHolding(false);
  }

  /**
   * 虚軸を描画します。
   * 
   * @param canvas キャンバス
   * @param min 虚部データの最小値
   * @param max 虚部データの最大値
   */
  private void drawImaginaryAxis(final Canvas canvas, final double min, final double max) {
    double imMin = min;
    double imMax = max;

    if (0.1 < Math.abs(imMax) || 0.1 < Math.abs(imMin)) {
      if (imMax < 1) {
        imMax = 1;
      } else if (100 < imMax) {
        imMax = 100;
      } else {
        imMax = Math.ceil(imMax / 10) * 10;
      }
      if (-1 < imMin) {
        imMin = -1;
      } else if (imMin < -100) {
        imMin = -100;
      } else {
        imMin = Math.floor(imMin / 10) * 10;
      }
    }

    // Imaginary axis
    canvas.setHolding(true);
    canvas.setYRange(imMin, imMax);
    final double grid = Math.pow(10, Math.floor(Math.log10((imMax - imMin) / 10))) * 2;
    canvas.setYTics(imMin, grid, imMax);
    canvas.plot(new DoubleMatrix(1, 2), new DoubleMatrix(new double[] {-imMax, imMax}), new String[] {""}, new String[0], new String[] {"with lines 1 0"}); //$NON-NLS-1$ //$NON-NLS-2$
    canvas.setHolding(false);
  }

  /**
   * メインメソッド
   * 
   * @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 DoubleNyquist(DoubleLinearSystemFactory.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;
  }
}