Bilinear.java

/*
 * $Id: Bilinear.java,v 1.15 2008/03/26 14:31:23 koga Exp $
 * 
 * Copyright (C) 2004 Koga Laboratory. All rights reserved.
 */
package org.mklab.tool.signal;

import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;

import org.mklab.nfc.matrix.DoubleMatrix;


/**
 * 双一次変換を行うクラスです。
 * 
 * <p> Bilinear transformation
 * 
 * @author koga
 * @version $Revision: 1.15 $
 * @see org.mklab.tool.control.C2d
 * @see org.mklab.tool.control.D2c
 */
public class Bilinear {

  /**
   * 連続時間伝達関数を離散時間伝達関数に双一次変換
   * 
   * <pre><code> G(z) = G(s) | | s = 2*fs*(z-1)/(z+1) </code></pre>
   * 
   * を用いて変換します。ただし、 <code>fs</code>はサンプリング周期[Hz]です。
   * 
   * @param A システム行列
   * @param B 入力行列
   * @param C 出力行列
   * @param D ゲイン行列
   * @param fs サンプリング周期
   * @return 変換結果のシステム (transformed system)
   */
  public static List<DoubleMatrix> bilinear(DoubleMatrix A, DoubleMatrix B, DoubleMatrix C, DoubleMatrix D, double fs) {
    // Bilinear transformation
    int n = A.getRowSize();
    double t = 1 / fs;
    double r = Math.sqrt(t);

    DoubleMatrix T1 = A.createUnit(n).add(A.multiply(t / 2));
    DoubleMatrix T2 = A.createUnit(n).subtract(A.multiply(t / 2));
    DoubleMatrix Ad = T2.leftDivide(T1);
    DoubleMatrix Bd = T2.leftDivide(B).multiply(t / r);
    DoubleMatrix Cd = C.divide(T2).multiply(r);
    DoubleMatrix Dd = C.divide(T2).multiply(B).multiply(t / 2).add(D);

    return new ArrayList<>(Arrays.asList(new DoubleMatrix[] {Ad, Bd, Cd, Dd}));
  }

  /**
   * 変換前後の周波数応答が周波数<code>fp</code>(Hz)で 正確に一致するよう、双一次変換の前に前処理を行います。
   * 
   * <p> Use prewarping before the bilinear transformation so that the frequency responses before and after mapping match exactly at the frequency <code>fp</code> (Hz).
   * 
   * @param A システム行列
   * @param B 入力行列
   * @param C 出力行列
   * @param D ゲイン行列
   * @param fs サンプリング周期[sec]
   * @param fp プリマッピングの周波数[Hz]
   * @return 変換結果のシステム (transformed system)
   */
  public static List<DoubleMatrix> bilinear(DoubleMatrix A, DoubleMatrix B, DoubleMatrix C, DoubleMatrix D, double fs, double fp) {
    double newFP = 2 * Math.PI * fp;
    double newFS = newFP / Math.tan(newFP / fs / 2) / 2;
    return bilinear(A, B, C, D, newFS);
  }

}