Margin.java

/*
 * $Id: Margin.java,v 1.17 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.scalar.DoubleNumber;


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

  /**
   * ゲイン余裕<code>gm</code>(絶対値), 位相余裕<code>pm</code>(度), ゲイン交差周波数<code>wgc</code>, 位相交差周波数<code>wpc</code>を返します。
   * 
   * @param Mg 周波数応答の絶対値(ゲイン)の列
   * @param Ph 周波数応答の偏角(位相)の列
   * @param w 周波数の列
   * @return {gm, pm, wgc, wpc} (ゲイン余裕, 位相余裕, ゲイン交差周波数, 位相交差周波数)
   */
  public static List<DoubleNumber> margin(DoubleMatrix Mg, DoubleMatrix Ph, DoubleMatrix w) {
    // Gain margin:
    IntMatrix idx;
    if (Ph.getDoubleElement(1) > -180.0) {
      idx = Ph.compareElementWise(".<", -180).find(); //$NON-NLS-1$
    } else {
      idx = Ph.compareElementWise(".>", -180).find(); //$NON-NLS-1$
    }

    double wpc, gm;

    if (idx.length() == 0 || idx.getIntElement(1) == 1) {
      System.err.println("Margin: " +  Messages.getString("Margin.0")); //$NON-NLS-1$ //$NON-NLS-2$
      wpc = Double.NaN;
      gm = Double.POSITIVE_INFINITY;
    } else {
      int j = idx.getIntElement(1);
      int i = j - 1;
      double wi = w.getDoubleElement(i);
      double wj = w.getDoubleElement(j);
      // linear approximation
      wpc = wi - (180 + Ph.getDoubleElement(i)) / (Ph.getDoubleElement(j) - Ph.getDoubleElement(i)) * (wj - wi);
      gm = 1.0 / (Mg.getDoubleElement(i) + (Mg.getDoubleElement(j) - Mg.getDoubleElement(i)) / (wj - wi) * (wpc - wi));
    }

    double wgc, pm;

    // Phase margin:
    if (Mg.getDoubleElement(1) > 1.0) {
      idx = Mg.compareElementWise(".<", 1.0).find(); //$NON-NLS-1$
    } else {
      idx = Mg.compareElementWise(".>", 1.0).find(); //$NON-NLS-1$
    }
    if (idx.length() == 0 || idx.getIntElement(1) == 1) {
      System.err.println("Margin: " +  Messages.getString("Margin.1")); //$NON-NLS-1$ //$NON-NLS-2$
      wgc = Double.NaN;
      pm = Double.POSITIVE_INFINITY;
    } else {
      int j = idx.getIntElement(1);
      int i = j - 1;
      double wi = w.getDoubleElement(i);
      double wj = w.getDoubleElement(j);
      // linear approximation
      wgc = wi + (1 - Mg.getDoubleElement(i)) / (Mg.getDoubleElement(j) - Mg.getDoubleElement(i)) * (wj - wi);
      pm = Ph.getDoubleElement(i) + (Ph.getDoubleElement(j) - Ph.getDoubleElement(i)) / (wj - wi) * (wgc - wi) + 180;
    }

    return new ArrayList<>(Arrays.asList(new DoubleNumber[] {new DoubleNumber(gm), new DoubleNumber(pm), new DoubleNumber(wgc), new DoubleNumber(wpc)}));
  }

}