Rlocus.java

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

import org.mklab.nfc.matrix.DoubleComplexMatrix;
import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.matrix.DoubleRationalPolynomialMatrix;
import org.mklab.nfc.matrix.misc.LogarithmicallySpacedVector;
import org.mklab.nfc.scalar.DoubleRationalPolynomial;
import org.mklab.tool.graph.gnuplot.Canvas;
import org.mklab.tool.graph.gnuplot.Gnuplot;


/**
 * 連続系の根軌跡を描画するためのデータを求めるクラスです。
 * 
 * <p>Root locus of MIMO cont-time linear ss. systems
 * 
 * @author koga
 * @version $Revision: 1.32 $
 * @see org.mklab.tool.control.Nyquist
 * @see org.mklab.tool.control.Bode
 */
public class Rlocus {

  /**
   * 根軌跡のデータを計算します。
   * 
   * @param A A行列
   * @param b B行列
   * @param c C行列
   * @param d D行列
   * @return 根軌跡(極の列) locus data
   */
  public static DoubleComplexMatrix rlocus(DoubleMatrix A, DoubleMatrix b, DoubleMatrix c, DoubleMatrix d) {
    DoubleMatrix K = LogarithmicallySpacedVector.create(-3.0, 3.0, 1000);
    return rlocus(A, b, c, d, K);
  }

  /**
   * システム
   * 
   * <pre><code>
   * 
   * . x = Ax + Bu y = Cx + Du u = -k*y
   * 
   * </code></pre>
   * 
   * の根軌跡のデータを返します。
   * 
   * @param A A行列
   * @param b B行列
   * @param c C行列
   * @param d D行列
   * @param K ゲインの列
   * @return 根軌跡(極の列) locus data
   */
  public static DoubleComplexMatrix rlocus(DoubleMatrix A, DoubleMatrix b, DoubleMatrix c, DoubleMatrix d, DoubleMatrix K) {
    String msg;
    if ((msg = Abcdchk.abcdchk(A, b, c, d)).length() > 0) {
      throw new RuntimeException(msg);
    }

    DoubleComplexMatrix RL = new DoubleComplexMatrix(A.getElement(1, 1).createZeroGrid(b.getRowSize(), K.length()));

    DoubleMatrix k = K.divideElementWise(K.multiply(d.getElement(1)).addElementWise(1));
    DoubleMatrix BC = b.multiply(c);
    for (int i = 1; i <= K.length(); i++) {
      RL.setColumnVector(i, A.subtract(BC.multiply(k.getElement(i))).eigenValue());
    }

    return RL;
  }

  /**
   * 根軌跡のデータを計算します。
   * 
   * @param g 伝達関数
   * @return 根軌跡(極の列) locus data
   */
  public static DoubleComplexMatrix rlocus(DoubleRationalPolynomial g) {
    List<DoubleMatrix> tmp = Tfn2ss.tfn2ss(g);
    DoubleMatrix A = tmp.get(0);
    DoubleMatrix b = tmp.get(1);
    DoubleMatrix c = tmp.get(2);
    DoubleMatrix d = tmp.get(3);
    return rlocus(A, b, c, d);
  }

  /**
   * 根軌跡のデータを計算します。
   * 
   * @param g 伝達関数
   * @param K ゲインの列
   * @return 根軌跡(極の列) locus data
   */
  public static DoubleComplexMatrix rlocus(DoubleRationalPolynomial g, DoubleMatrix K) {
    List<DoubleMatrix> tmp = Tfn2ss.tfn2ss(g);
    DoubleMatrix A = tmp.get(0);
    DoubleMatrix b = tmp.get(1);
    DoubleMatrix c = tmp.get(2);
    DoubleMatrix d = tmp.get(3);
    return rlocus(A, b, c, d, K);
  }

  /**
   * 根軌跡のデータを計算します。
   * 
   * @param G 伝達関数行列
   * @return 根軌跡(極の列) locus data
   */
  public static DoubleComplexMatrix rlocus(DoubleRationalPolynomialMatrix G) {
    List<DoubleMatrix> tmp = Tfm2ss.tfm2ss(G);
    DoubleMatrix A = tmp.get(0);
    DoubleMatrix b = tmp.get(1);
    DoubleMatrix c = tmp.get(2);
    DoubleMatrix d = tmp.get(3);
    return rlocus(A, b, c, d);
  }

  /**
   * 根軌跡のデータを計算します。
   * 
   * @param G 伝達関数行列
   * @param K ゲインの列
   * @return 根軌跡(極の列) locus data
   */
  public static DoubleComplexMatrix rlocus(DoubleRationalPolynomialMatrix G, DoubleMatrix K) {
    List<DoubleMatrix> tmp = Tfm2ss.tfm2ss(G);
    DoubleMatrix A = tmp.get(0);
    DoubleMatrix b = tmp.get(1);
    DoubleMatrix c = tmp.get(2);
    DoubleMatrix d = tmp.get(3);
    return rlocus(A, b, c, d, K);
  }

  /**
   * 方程式
   * 
   * <pre><code>
   * 
   * num(s) H(s) = 1 + k ---------- = 0 den(s)
   * 
   * </code></pre>
   * 
   * の根の軌跡データを返します。
   * 
   * <p>Returns the locus of the zeros of
   * 
   * <pre><code>
   * 
   * num(s) H(s) = 1 + k ---------- = 0 den(s)
   * 
   * </code></pre>
   * 
   * or the gains specified in <code>K</code>.
   * 
   * @param numerator 伝達関数の分子多項式の係数
   * @param denominator 伝達関数の分母多項式の係数
   * @return 根軌跡(極の列) locus data
   */
  public static DoubleComplexMatrix rlocus(DoubleMatrix numerator, DoubleMatrix denominator) {
    List<DoubleMatrix> tmp = Tf2ss.tf2ss(numerator, denominator);
    DoubleMatrix A = tmp.get(0);
    DoubleMatrix b = tmp.get(1);
    DoubleMatrix c = tmp.get(2);
    DoubleMatrix d = tmp.get(3);
    return rlocus(A, b, c, d);
  }

  /**
   * 各列が <code>K(i)</code> に対応する行列を返します。
   * 
   * <p>Returns a (length(den)-1)-by-length(K) matrix. Each column RL(:,i) corresponds to a gain K(i).
   * 
   * @param numerator 伝達関数の分子多項式の係数
   * @param denominator 伝達関数の分母多項式の係数
   * @param K ゲインの列
   * @return 根軌跡(極の列) locus data
   */
  public static DoubleComplexMatrix rlocus(DoubleMatrix numerator, DoubleMatrix denominator, DoubleMatrix K) {
    List<DoubleMatrix> tmp = Tf2ss.tf2ss(numerator, denominator);
    DoubleMatrix A = tmp.get(0);
    DoubleMatrix b = tmp.get(1);
    DoubleMatrix c = tmp.get(2);
    DoubleMatrix d = tmp.get(3);
    return rlocus(A, b, c, d, K);
  }

  /**
   * @param A A行列
   * @param b B行列
   * @param c C行列
   * @param d D行列
   * @return Gnuplot
   * @throws IOException gnuplotプロセスを起動できない場合
   */
  public static Gnuplot plot(DoubleMatrix A, DoubleMatrix b, DoubleMatrix c, DoubleMatrix d) throws IOException {
    Gnuplot gp = new Gnuplot();
    return plot(gp, A, b, c, d);
  }

  /**
   * 根軌跡をプロットします。
   * 
   * @param gnuplot gnuplot
   * @param A A行列
   * @param b B行列
   * @param c C行列
   * @param d D行列
   * @return Gnuplot
   */
  public static Gnuplot plot(Gnuplot gnuplot, DoubleMatrix A, DoubleMatrix b, DoubleMatrix c, DoubleMatrix d) {
    DoubleMatrix K = LogarithmicallySpacedVector.create(-3.0, 3.0, 1000);
    return plot(gnuplot, A, b, c, d, K);
  }

  /**
   * @param A A行列
   * @param b B行列
   * @param c C行列
   * @param d D行列
   * @param K ゲインの列
   * @return Gnuplot
   * @throws IOException gnuplotプロセスを起動できない場合
   */
  public static Gnuplot plot(DoubleMatrix A, DoubleMatrix b, DoubleMatrix c, DoubleMatrix d, DoubleMatrix K) throws IOException {
    Gnuplot gp = new Gnuplot();
    return plot(gp, A, b, c, d, K);
  }

  /**
   * 根軌跡をプロットします。
   * 
   * @param gnuplot gnuplot
   * @param A A行列
   * @param b B行列
   * @param c C行列
   * @param d D行列
   * @param K ゲインの列
   * @return Gnuplot
   */
  public static Gnuplot plot(Gnuplot gnuplot, DoubleMatrix A, DoubleMatrix b, DoubleMatrix c, DoubleMatrix d, DoubleMatrix K) {
    Canvas canvas = gnuplot.createCanvas();
    canvas.setGridVisible(true);
    canvas.setKeyVisible(false);
    canvas.doCommand("set data style points"); //$NON-NLS-1$
    canvas.setTitle("Root-locus"); //$NON-NLS-1$
    canvas.setXLabel("Re"); //$NON-NLS-1$
    canvas.setYLabel("Im"); //$NON-NLS-1$
    DoubleComplexMatrix R = rlocus(A, b, c, d, K);
    canvas.plot(R.getRealPart(), R.getImaginaryPart());
    return gnuplot;
  }

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

  /**
   * 根軌跡をプロットします。
   * 
   * @param gnuplot gnuplot
   * @param g 伝達関数
   * @return Gnuplot
   */
  public static Gnuplot plot(Gnuplot gnuplot, DoubleRationalPolynomial g) {
    DoubleMatrix K = LogarithmicallySpacedVector.create(-3.0, 3.0, 1000);
    return plot(gnuplot, g, K);
  }

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

  /**
   * 根軌跡をプロットします。
   * 
   * @param gnuplot gnuplot
   * @param g 伝達関数
   * @param K ゲインの列
   * @return Gnuplot
   */
  public static Gnuplot plot(Gnuplot gnuplot, DoubleRationalPolynomial g, DoubleMatrix K) {
    List<DoubleMatrix> tmp = Tfn2ss.tfn2ss(g);
    DoubleMatrix A = tmp.get(0);
    DoubleMatrix b = tmp.get(1);
    DoubleMatrix c = tmp.get(2);
    DoubleMatrix d = tmp.get(3);
    return plot(gnuplot, A, b, c, d, K);
  }

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

  /**
   * 根軌跡をプロットします。
   * 
   * @param gnuplot gnuplot
   * @param G 伝達関数行列
   * @return Gnuplot
   */
  public static Gnuplot plot(Gnuplot gnuplot, DoubleRationalPolynomialMatrix G) {
    DoubleMatrix K = LogarithmicallySpacedVector.create(-3.0, 3.0, 1000);
    return plot(gnuplot, G, K);
  }

  /**
   * @param G 伝達関数行列
   * @param K ゲインの列
   * @return Gnuplot
   * @throws IOException gnuplotプロセスを起動できない場合
   */
  public static Gnuplot plot(DoubleRationalPolynomialMatrix G, DoubleMatrix K) throws IOException {
    Gnuplot gp = new Gnuplot();
    return plot(gp, G, K);
  }

  /**
   * 根軌跡をプロットします。
   * 
   * @param gnuplot gnuplot
   * @param G 伝達関数行列
   * @param K ゲインの列
   * @return Gnuplot
   */
  public static Gnuplot plot(Gnuplot gnuplot, DoubleRationalPolynomialMatrix G, DoubleMatrix K) {
    List<DoubleMatrix> tmp = Tfm2ss.tfm2ss(G);
    DoubleMatrix A = tmp.get(0);
    DoubleMatrix b = tmp.get(1);
    DoubleMatrix c = tmp.get(2);
    DoubleMatrix d = tmp.get(3);
    return plot(gnuplot, A, b, c, d, K);
  }

  /**
   * @param numerator 伝達関数の分子多項式の係数
   * @param denominator 伝達関数の分母多項式の係数
   * @return Gnuplot
   * @throws IOException gnuplotプロセスを起動できない場合
   */
  public static Gnuplot plot(DoubleMatrix numerator, DoubleMatrix denominator) throws IOException {
    Gnuplot gp = new Gnuplot();
    return plot(gp, numerator, denominator);
  }

  /**
   * 根軌跡をプロットします。
   * 
   * @param gnuplot gnuplot
   * @param numerator 伝達関数の分子多項式の係数
   * @param denominator 伝達関数の分母多項式の係数
   * @return Gnuplot
   */
  public static Gnuplot plot(Gnuplot gnuplot, DoubleMatrix numerator, DoubleMatrix denominator) {
    DoubleMatrix K = LogarithmicallySpacedVector.create(-3.0, 3.0, 1000);
    return plot(gnuplot, numerator, denominator, K);
  }

  /**
   * @param numerator 伝達関数の分子多項式の係数
   * @param denominator 伝達関数の分母多項式の係数
   * @param K ゲインの列
   * @return Gnuplot
   * @throws IOException gnuplotプロセスを起動できない場合
   */
  public static Gnuplot plot(DoubleMatrix numerator, DoubleMatrix denominator, DoubleMatrix K) throws IOException {
    Gnuplot gp = new Gnuplot();
    return plot(gp, numerator, denominator, K);
  }

  /**
   * 根軌跡をプロットします。
   * 
   * @param gnuplot gnuplot
   * @param numerator 伝達関数の分子多項式の係数
   * @param denominator 伝達関数の分母多項式の係数
   * @param K ゲインの列
   * @return Gnuplot
   */
  public static Gnuplot plot(Gnuplot gnuplot, DoubleMatrix numerator, DoubleMatrix denominator, DoubleMatrix K) {
    List<DoubleMatrix> tmp = Tf2ss.tf2ss(numerator, denominator);
    DoubleMatrix A = tmp.get(0);
    DoubleMatrix b = tmp.get(1);
    DoubleMatrix c = tmp.get(2);
    DoubleMatrix d = tmp.get(3);
    return plot(gnuplot, A, b, c, d, K);
  }

}