Pmargin.java

/*
 * $Id: Pmargin.java,v 1.38 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.DoubleComplexMatrix;
import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.matrix.IntMatrix;
import org.mklab.nfc.matrix.misc.LogarithmicallySpacedVector;
import org.mklab.nfc.scalar.DoubleComplexNumber;
import org.mklab.nfc.scalar.DoubleComplexPolynomial;
import org.mklab.nfc.scalar.DoubleComplexRationalPolynomial;
import org.mklab.nfc.scalar.DoubleNumber;
import org.mklab.nfc.scalar.DoublePolynomial;
import org.mklab.nfc.scalar.DoubleRationalPolynomial;
import org.mklab.tool.matrix.Unwrap;


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

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

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

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

  /**
   * 位相余裕 <code>pm</code> とゲイン交差周波数 <code>wcg</code> を返します。
   * 
   * @param g 伝達関数
   * @param wmin 最小周波数
   * @param wmax 最大周波数
   * @param tolerance ゲイン交差周波数の許容誤差
   * @return {pm, wcg} (位相余裕, ゲイン交差周波数) margin
   */
  public static List<DoubleNumber> pmargin(DoubleRationalPolynomial g, double wmin, double wmax, double tolerance) {
    double wmin2 = wmin;
    double wmax2 = wmax;

    double gainCrossFrequency, phaseMargin;
    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 = gain.compareElementWise(".<", 0.0).find(); //$NON-NLS-1$
      if (idx.length() == 0) {
        System.err.println("Pmargin: " + Messages.getString("Pmargin.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("Pmargin: " + Messages.getString("Pmargin.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) {
        gainCrossFrequency = wmax2;
        phaseMargin = 180 + phase.getDoubleElement(idx.getIntElement(1));
        break;
      }
    }

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

  /**
   * @param g 伝達関数
   * @return {pm, wcg} (位相余裕, ゲイン交差周波数) margin
   */
  public static List<DoubleNumber> pmargin_roots(DoubleRationalPolynomial g) {
    double tolerance = 1.0E-12;
    return pmargin_roots(g, tolerance);
  }

  /**
   * @param g 伝達関数
   * @param tolerance ゲイン交差周波数の許容誤差
   * @return {pm, wcg} (位相余裕, ゲイン交差周波数) margin
   */
  public static List<DoubleNumber> pmargin_roots(DoubleRationalPolynomial g, double tolerance) {
    DoubleComplexNumber j = new DoubleComplexNumber(0, 1);

    DoublePolynomial w = new DoublePolynomial("s"); //$NON-NLS-1$
    DoubleComplexPolynomial nc = new DoubleComplexPolynomial(g.getNumerator()).evaluate(w.multiply(j));
    DoubleComplexPolynomial dc = new DoubleComplexPolynomial(g.getDenominator()).evaluate(w.multiply(j));
    DoublePolynomial eq = nc.getRealPart().power(2).add(nc.getImaginaryPart().power(2)).subtract(dc.getRealPart().power(2).add(dc.getImaginaryPart().power(2)));

    DoubleComplexMatrix rt = eq.getRoots();
    DoubleMatrix wcgs = (rt
        .getSubVector((rt.getSubVector(rt.getImaginaryPart().absElementWise().compareElementWise(".<", tolerance).find())).getRealPart().compareElementWise(".>", //$NON-NLS-1$ //$NON-NLS-2$
            0.0).find())).getRealPart();

    DoubleNumber wcg;

    if (wcgs.length() == 0) { // no crossover frequency
      System.err.println("Pmargin: " + Messages.getString("Pmargin.5")); //$NON-NLS-1$ //$NON-NLS-2$
      wcg = new DoubleNumber(-1);
    } else {
      wcg = wcgs.getElement(1);
    }

    if (wcg.isLessThan(0)) {
      return Arrays.<DoubleNumber> asList(new DoubleNumber[]{new DoubleNumber(0), new DoubleNumber(0)});
    }
    return Arrays.<DoubleNumber> asList(new DoubleNumber[] {(new DoubleComplexRationalPolynomial(g).evaluate(j.multiply(new DoubleComplexNumber(wcg)))).arg().multiply(180).divide(Math.PI), wcg});
  }

}