Nichols.java

/*
 * $Id: Nichols.java,v 1.39 2008/03/08 00:17:41 koga Exp $
 *
 * Copyright (C) 2004 Koga Laboratory. All rights reserved.
 */

package org.mklab.tool.control;

import java.io.IOException;
import java.util.List;

import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.matrix.misc.LinearlySpacedVector;
import org.mklab.nfc.matrix.misc.LogarithmicallySpacedVector;
import org.mklab.nfc.scalar.DoubleNumberUtil;
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>Nichols plot of continuous time linear system
 * 
 * @author koga
 * @version $Revision: 1.39 $
 */
public class Nichols {

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

    try {
      Pause.pause();
    } finally {
      gnuplot.close();
    }
  }

  /**
   * @param g 伝達関数行列
   * @return Gnuplot
   * @throws IOException gnuplotプロセスを起動できない場合
   */
  public static Gnuplot plot(DoubleRationalPolynomial g) throws IOException {
    final Gnuplot gnuplot = new Gnuplot();
    return plot(gnuplot, g);
  }

  /**
   * ニコルス線図をプロットします。
   * 
   * @param gnuplot gnuplot
   * @param g 伝達関数
   * @return Gnuplot
   */
  public static Gnuplot plot(Gnuplot gnuplot, DoubleRationalPolynomial g) {
    final DoubleMatrix w = LogarithmicallySpacedVector.create(-2.0, 1.0);
    return plot(gnuplot, g, w);
  }

  /**
   * @param g 伝達関数
   * @param w 評価する周波数
   * @return Gnuplot
   * @throws IOException gnuplotプロセスを起動できない場合
   */
  public static Gnuplot plot(DoubleRationalPolynomial g, DoubleMatrix w) throws IOException {
    final Gnuplot gnuplot = new Gnuplot();
    return plot(gnuplot, g, w);
  }

  /**
   * ニコルス線図をプロットします。
   * 
   * @param gnuplot gnuplot
   * @param g 伝達関数
   * @param w 評価する周波数
   * @return Gnuplot
   */
  @SuppressWarnings("nls")
  public static Gnuplot plot(Gnuplot gnuplot, DoubleRationalPolynomial g, DoubleMatrix w) {
    final List<DoubleMatrix> gainPhase = new DoubleBode(DoubleLinearSystemFactory.createLinearSystem(g)).getGainAndPhase(w).get(0);
    DoubleMatrix gain = gainPhase.get(0);
    DoubleMatrix phase = gainPhase.get(1);

    gain = gain.log10ElementWise().multiply(20);
    double gain_max = Math.ceil(gain.divide(5).max().doubleValue()) * 5;
    double gain_min = Math.floor(gain.divide(5).min().doubleValue()) * 5;
    // double phase_max = Math.ceil(((RealMatrix)G_phase.divide(90)).max()+1) *
    // 90;
    // double phase_min = Math.floor(((RealMatrix)G_phase.divide(90)).min()-1) *
    // 90;
    if (gain_max < 40.0) {
      gain_max = 40.0;
    }
    if (-30.0 < gain_min) {
      gain_min = -30.0;
    }

    // Constant M loci

    // M = 0
    DoubleMatrix theta_0 = LinearlySpacedVector.create(-269.0, -91.0, 50); // deg
    DoubleMatrix G_M0 = new DoubleMatrix(1, theta_0.length());
    for (int j = 1; j <= theta_0.length(); j++) {
      double ca = Math.cos(theta_0.getDoubleElement(j) / 180 * Math.PI);
      G_M0.setElement(1, j, -1 / (2 * ca));
    }

    // M < 0
    DoubleMatrix Mm = new DoubleMatrix(new double[] {0.5, 1, 2, 4, 6, 10, 15, 20, 27, 33, 40}).unaryMinus(); // dB
    DoubleMatrix theta_m = LinearlySpacedVector.create(-360.0, 0.0, 50); // deg
    int Nm = Mm.length();
    DoubleMatrix G_Mm = new DoubleMatrix(Nm, theta_m.length());

    for (int i = 1; i <= Nm; i++) {
      double m = Math.pow(10, (Mm.getDoubleElement(i) / 20));
      double m2 = m * m;
      for (int j = 1; j <= theta_m.length(); j++) {
        double ca = Math.cos(theta_m.getDoubleElement(j) / 180 * Math.PI);
        G_Mm.setElement(i, j, (m2 / (m2 - 1)) * (-ca - Math.sqrt(ca * ca - 1 + 1 / m2)));
      }
    }

    // M > 0
    DoubleMatrix Mp = new DoubleMatrix(new double[] {12, 6, 4, 3, 2, 1, 0.5, 0.25}); // dB
    DoubleMatrix theta_p = LinearlySpacedVector.create(-270.0, -90.0); // deg
    theta_p = theta_p.appendRight(theta_p.flipLeftRight()); // deg
    int Np = Mp.length();
    DoubleMatrix G_Mp = new DoubleMatrix(Np, theta_p.length());
    DoubleMatrix Theta_p = new DoubleMatrix(Np, theta_p.length());

    for (int i = 1; i <= Np; i++) {
      double m = Math.pow(10, (Mp.getDoubleElement(i) / 20));
      double m2 = m * m;
      for (int j = 1; j <= theta_p.length(); j++) {
        double ca = Math.cos(theta_p.getDoubleElement(j) / 180 * Math.PI);
        if (j <= theta_p.length() / 2) {
          if (ca * ca - 1 + 1 / m2 >= 0.0) {
            Theta_p.setElement(i, j, theta_p.getDoubleElement(j));
            G_Mp.setElement(i, j, (m2 / (m2 - 1)) * (-ca + Math.sqrt(ca * ca - 1 + 1 / m2)));
          } else {
            if (j != 1) {
              Theta_p.setElement(i, j, Theta_p.getDoubleElement(i, j - 1));
              G_Mp.setElement(i, j, G_Mp.getDoubleElement(i, j - 1));
            } else {
              Theta_p.setElement(i, j, 0.0); // temporary point
              G_Mp.setElement(i, j, 0.0); // temporary point
            }
          }
        } else {
          if (ca * ca - 1 + 1 / m2 >= 0.0) {
            Theta_p.setElement(i, j, theta_p.getDoubleElement(j));
            G_Mp.setElement(i, j, (m2 / (m2 - 1)) * (-ca - Math.sqrt(ca * ca - 1 + 1 / m2)));
          } else {
            Theta_p.setElement(i, j, Theta_p.getDoubleElement(i, j - 1));
            G_Mp.setElement(i, j, G_Mp.getDoubleElement(i, j - 1));
          }
        }
      }
      // Change the temporary points
      for (int j = 1; j <= theta_p.length() / 2; j++) {
        if (Theta_p.getDoubleElement(i, j) == 0.0 && G_Mp.getDoubleElement(i, j) == 0.0) {
          Theta_p.setElement(i, j, Theta_p.getDoubleElement(i, theta_p.length()));
          G_Mp.setElement(i, j, G_Mp.getDoubleElement(i, theta_p.length()));
        }
      }
    }

    //
    // Constant alpha loci
    //
    DoubleMatrix Alpha = new DoubleMatrix(new double[] {150, 120, 90, 60, 30, 20, 10, 5, 2});
    Alpha = Alpha.unaryMinus().appendRight(Alpha.flipLeftRight());
    DoubleMatrix theta = LinearlySpacedVector.create(-360.0, 0.0);
    int Na = Alpha.length();
    DoubleMatrix Ga = new DoubleMatrix(Na, theta.length());
    DoubleMatrix Theta_a = new DoubleMatrix(Na, theta.length());
    for (int i = 1; i <= Alpha.length(); i++) {
      double al = Alpha.getDoubleElement(i) / 180 * Math.PI;
      for (int j = 1; j <= theta.length(); j++) {
        double th = theta.getDoubleElement(j) / 180 * Math.PI;
        Ga.setElement(i, j, (1 / Math.tan(al)) * Math.sin(th) - Math.cos(th));
        Theta_a.setElement(i, j, theta.getDoubleElement(j));
        if (Ga.getDoubleElement(i, j) < 0.0) {
          Ga.setElement(i, j, DoubleNumberUtil.EPS);
          Theta_a.setElement(i, j, theta.getDoubleElement(j));
        }
      }
    }

    String[] data_tits_p = new String[Np];
    String[] data_cmds_p = new String[Np];
    for (int i = 1; i <= Np; i++) {
      data_tits_p[i - 1] = "";
      data_cmds_p[i - 1] = "2";
    }

    String[] data_tits_m = new String[Nm];
    String[] data_cmds_m = new String[Nm];
    for (int i = 1; i <= Nm; i++) {
      data_tits_m[i - 1] = "";
      data_cmds_m[i - 1] = "2";
    }

    String[] data_tits_a = new String[Na];
    String[] data_cmds_a = new String[Na];
    for (int i = 1; i <= Na; i++) {
      data_tits_a[i - 1] = "";
      data_cmds_a[i - 1] = "3";
    }

    Canvas canvas = gnuplot.createCanvas();
    canvas.setGridVisible(true);
    canvas.setXLabel("phase [deg]");
    canvas.setYLabel("gain [dB]");
    canvas.setXRange(-360, 0);
    canvas.setXTics(-360, 60, 0);
    canvas.setYRange(gain_min, gain_max);

    canvas.setText("0dB", -100.0, 27.0, "center");
    canvas.setText("-0.5dB", -5.0, 26.0, "right");
    canvas.setText("-1dB", -5.0, 19.0, "right");
    canvas.setText("-2dB", -5.0, 13.0, "right");
    canvas.setText("-4dB", -5.0, 6.0, "right");
    canvas.setText("-6dB", -5.0, 1.5, "right");
    canvas.setText("-10dB", -5.0, -5.5, "right");
    canvas.setText("-15dB", -5.0, -12.0, "right");
    canvas.setText("-20dB", -5.0, -18.0, "right");
    canvas.setText("-27dB", -5.0, -25.0, "right");

    canvas.setText("0.25dB", -180.0, 32.0, "center");
    canvas.setText("0.5dB", -180.0, 26.0, "center");
    canvas.setText("1dB", -180.0, 20.0, "center");
    canvas.setText("2dB", -180.0, 15.0, "center");
    canvas.setText("3dB", -180.0, 11.5, "center");
    canvas.setText("4dB", -180.0, 9.5, "center");
    canvas.setText("6dB", -180.0, 7.0, "center");
    canvas.setText("12dB", -180.0, 3.5, "center");

    canvas.setText("-2deg", -70.0, 30.0, "center");
    canvas.setText("-5deg", -70.0, 22.0, "center");
    canvas.setText("-10deg", -70.0, 15.0, "center");
    canvas.setText("-20deg", -70.0, 9.0, "center");
    canvas.setText("-30deg", -70.0, 4.0, "center");

    canvas.setText("-60deg", -82.0, -3.0, "center");
    canvas.setText("-90deg", -100.0, -7.0, "center");
    canvas.setText("-120deg", -120.0, -10.0, "center");
    canvas.setText("-150deg", -145.0, -13.0, "center");

    canvas.setText("+2deg", -290.0, 30.0, "center");
    canvas.setText("+5deg", -290.0, 22.0, "center");
    canvas.setText("+10deg", -290.0, 15.0, "center");
    canvas.setText("+20deg", -290.0, 9.0, "center");
    canvas.setText("+30deg", -290.0, 4.0, "center");

    canvas.setText("+60deg", -278.0, -3.0, "center");
    canvas.setText("+90deg", -260.0, -7.0, "center");
    canvas.setText("+120deg", -240.0, -10.0, "center");
    canvas.setText("+150deg", -215.0, -13.0, "center");

    canvas.plot(phase, gain, new String[] {"G(s)"}, new String[0], new String[] {"1"});
    canvas.setHolding(true);
    canvas.plot(new DoubleMatrix(1, 1), new DoubleMatrix(1, 1), new String[] {"alpha loci"}, new String[0], new String[] {"3"});
    canvas.plot(theta_0, G_M0.absElementWise().log10ElementWise().multiply(20), new String[] {"M loci"}, new String[0], new String[] {"2"});
    canvas.plot(theta_m, G_Mm.absElementWise().log10ElementWise().multiply(20), data_tits_m, new String[0], data_cmds_m);
    canvas.plot(Theta_p, G_Mp.absElementWise().log10ElementWise().multiply(20), data_tits_p, new String[0], data_cmds_p);
    canvas.plot(Theta_a, Ga.absElementWise().log10ElementWise().multiply(20), data_tits_a, new String[0], data_cmds_a);
    canvas.setHolding(false);
    return gnuplot;
  }

}