Zp2tf.java

/*
 * $Id: Zp2tf.java,v 1.34 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.tool.matrix.Makecolv;
import org.mklab.tool.matrix.Makepoly;


/**
 * 極とゼロ点の集合から伝達関数(係数からなる行列)に変換するクラスです。
 * 
 * <p>Zero-pole to transfer function conversion
 * 
 * @author koga
 * @version $Revision: 1.34 $
 * @see org.mklab.tool.control.Zp2tfm
 * @see org.mklab.tool.control.Zp2tfn
 * @see org.mklab.tool.control.Zp2ss
 * @see org.mklab.tool.control.Tf2zp
 */
public class Zp2tf {

  /**
   * 極<code>p</code>、ゼロ点<code>z</code>,ゲイン<code>k</code>である1入力多出力システム
   * 
   * <pre><code> NUM(s) (s-z1)(s-z2)...(s-zn) G(s) = ------ = k --------------------- den(s) (s-p1)(s-p2)...(s-pn) </code></pre>
   * 
   * の伝達関数行列の分子行列多項式の係数行列と分母多項式の係数ベクトルを返します。
   * 
   * <p><code>NUM</code>の大きさは、<code>Rows(z)×(length(p)-1)</code> <code>den</code>の大きさは、<code>1×(length(p)-1)</code>です。
   * 
   * @param zeros ゼロ点の集合
   * @param poles 極の集合
   * @param k ゲイン
   * @return 伝達関数 (transfer function)
   */
  public static List<DoubleMatrix> zp2tf(DoubleComplexMatrix zeros, DoubleComplexMatrix poles, DoubleMatrix k) {
    final DoubleMatrix denCoef = Makepoly.makepoly(poles).getCoefficients().flipLeftRight();
    final DoubleMatrix den;
//    if (denCoef.isComplex()) {
//      den = denCoef.getRealPart();
//    } else {
//      den = denCoef;
//    }
    
    den = denCoef;

    final int stateSize = zeros.getColumnSize();
    final int outputSize = zeros.getRowSize();

    if (stateSize == 0 || outputSize == 0) {
      final DoubleMatrix NUM = Makecolv.makecolv(k);
      return new ArrayList<>(Arrays.asList(new DoubleMatrix[] {NUM.flipLeftRight(), den.flipLeftRight()}));
    }

    if (outputSize != k.length()) {
      throw new RuntimeException(Messages.getString("Zp2tf.0")); //$NON-NLS-1$
    }

    final DoubleMatrix NUM = k.createZero(outputSize, stateSize + 1);

    for (int i = 1; i <= outputSize; i++) {
      DoubleComplexMatrix zz = zeros.getRowVector(i);
      zz = zz.getSubVector(zz.absElementWise().isInfiniteElementWise().notElementWise().find());
      final DoubleMatrix nuCoef = Makepoly.makepoly(zz).getCoefficients().flipLeftRight().multiply(k.getElement(i));
      final DoubleMatrix nu;
//      if (nuCoef.isComplex()) {
//        nu = nuCoef.getRealPart();
//      } else {
//        nu = nuCoef;
//      }
      nu = nuCoef;
      
      NUM.setSubMatrix(i, i, 1, NUM.getColumnSize(), k.createZero(1, stateSize + 1 - nu.length()).appendRight(nu));
    }
    return new ArrayList<>(Arrays.asList(new DoubleMatrix[] {NUM.flipLeftRight(), den.flipLeftRight()}));
  }

}