Gmargin.java

/*
 * $Id: Gmargin.java,v 1.29 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.Arrays;
import java.util.List;

import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.matrix.IntMatrix;
import org.mklab.nfc.matrix.misc.LogarithmicallySpacedVector;
import org.mklab.nfc.scalar.DoubleNumber;
import org.mklab.nfc.scalar.DoubleRationalPolynomial;
import org.mklab.tool.matrix.Unwrap;


/**
 * ゲイン余裕と位相交差周波数を求めるクラスです。
 * 
 * <p>Gain margin and crossover frequencies
 * 
 * @author koga
 * @version $Revision: 1.29 $
 * @see org.mklab.tool.control.Pmargin
 * @see org.mklab.tool.control.Margin
 */
public class Gmargin {

  /**
   * @param G 伝達関数
   * @return {gm, wcp} (ゲイン余裕, 位相交差周波数) (gain margin, crossover frequency)
   */
  public static List<DoubleNumber> gmargin(DoubleRationalPolynomial G) {
    double wmin = 0.001;
    double wmax = 1000.0;
    double tolerance = 1.0E-3;

    return gmargin(G, wmin, wmax, tolerance);
  }

  /**
   * @param G 伝達関数
   * @param wmin 最小周波数
   * @return {gm, wcp} (ゲイン余裕, 位相交差周波数) (gain margin, crossover frequency)
   */
  public static List<DoubleNumber> gmargin(DoubleRationalPolynomial G, double wmin) {
    double wmax = 1000.0;
    double tolerance = 1.0E-3;

    return gmargin(G, wmin, wmax, tolerance);
  }

  /**
   * @param G 伝達関数
   * @param wmin 最小周波数
   * @param wmax 最大周波数
   * @return {gm, wcp} (ゲイン余裕, 位相交差周波数) (gain margin, crossover frequency)
   */
  public static List<DoubleNumber> gmargin(DoubleRationalPolynomial G, double wmin, double wmax) {
    double tolerance = 1.0E-3;

    return gmargin(G, wmin, wmax, tolerance);
  }

  /**
   * 与えられた伝達関数<code>G</code>, 周波数の区間<code>[wmin:wmax]</code>, 周波数の許容誤差<code>tolerance</code>について, ゲイン余裕 <code>gm</code>と位相交差周波数<code>wcp</code>を返します。
   * 
   * @param g 伝達関数
   * @param wmin 最小周波数
   * @param wmax 最大周波数
   * @param tolerance 位相交差周波数の許容誤差
   * @return {gm, wcp} (ゲイン余裕, 位相交差周波数) (gain margin, crossover frequency)
   */
  public static List<DoubleNumber> gmargin(DoubleRationalPolynomial g, double wmin, double wmax, double tolerance) {
    double wmin2 = wmin;
    double wmax2 = wmax;

    double phaseCrossFrequency;
    double gainMargin;

    for (;;) {
      final DoubleMatrix w = LogarithmicallySpacedVector.create(Math.log(wmin2) / Math.log(10), Math.log(wmax2) / Math.log(10), 100);
      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);
      phase = Unwrap.unwrapRowWise(phase);

      IntMatrix idx = phase.compareElementWise(".<", -180.0).find(); //$NON-NLS-1$

      if (idx.length() == 0) {
        System.err.println("Gmargin: " + Messages.getString("Gmargin.0")); //$NON-NLS-1$ //$NON-NLS-2$
        return new ArrayList<>(Arrays.asList(new DoubleNumber[] {new DoubleNumber(Double.POSITIVE_INFINITY), new DoubleNumber(Double.NaN)}));
      } else if (idx.length() == w.getColumnSize()) {
        System.err.println("Gmargin: " + Messages.getString("Gmargin.1")); //$NON-NLS-1$ //$NON-NLS-2$
        return new ArrayList<>(Arrays.asList(new DoubleNumber[] {new DoubleNumber(Double.POSITIVE_INFINITY), new DoubleNumber(Double.NaN)}));
      }

      wmin2 = w.getDoubleElement(idx.getIntElement(1) - 1);
      wmax2 = w.getDoubleElement(idx.getIntElement(1));

      if (wmax2 - wmin2 < tolerance) {
        phaseCrossFrequency = wmax2;
        gainMargin = -gain.getDoubleElement(idx.getIntElement(1));
        break;
      }
    }

    return new ArrayList<>(Arrays.asList(new DoubleNumber[] {new DoubleNumber(gainMargin), new DoubleNumber(phaseCrossFrequency)}));
  }

}