D2c.java

/*
 * $Id: D2c.java,v 1.23 2008/03/23 14:29:07 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.scalar.DoubleNumber;
import org.mklab.tool.matrix.Matrix4;


/**
 * 離散時間系を対応する連続時間系に変換するクラスです。
 * 
 * <p>Discrete-time to continuous-time transformation
 * 
 * @author koga
 * @version $Revision: 1.23 $
 * @see org.mklab.tool.control.C2d
 */
public class D2c {

  /**
   * 離散時間システム
   * 
   * <pre><code> x[n + 1] = Ad * x[n] + Bd * u[n] </code></pre>
   * 
   * を連続時間システム
   * 
   * <pre><code> dx/dt = Ax + Bu </code></pre>
   * 
   * に変換します。
   * 
   * ただし、入力はゼロ次ホールドを通して加えられるとします。
   * 
   * @param Ad 離散時間系システム行列
   * @param Bd 離散時間系の入力行列
   * @param samplingInterval サンプリング周期
   * @return {A, B} (連続時間系)(continuous-time system)
   */
  public static List<DoubleMatrix> d2c(DoubleMatrix Ad, DoubleMatrix Bd, double samplingInterval) {
    return d2c(Ad, Bd, new DoubleNumber(samplingInterval));
  }

  /**
   * 離散時間システム
   * 
   * <pre><code> x[n + 1] = Ad * x[n] + Bd * u[n] </code></pre>
   * 
   * を連続時間システム
   * 
   * <pre><code> dx/dt = Ax + Bu </code></pre>
   * 
   * に変換します。
   * 
   * ただし、入力はゼロ次ホールドを通して加えられるとします。
   * 
   * @param Ad 離散時間系のシステム行列
   * @param Bd 離散時間系の入力行列
   * @param samplingInterval サンプリング周期
   * @return {A, B} (連続時間系)(continuous-time system)
   */
  public static List<DoubleMatrix> d2c(DoubleMatrix Ad, DoubleMatrix Bd, DoubleNumber samplingInterval) {
    final String message;
    if ((message = Abcdchk.abcdchk(Ad, Bd)).length() > 0) {
      throw new IllegalArgumentException(message);
    }

    final int stateSize = Ad.getRowSize();
    final int inputSize = Bd.getColumnSize();

    if (stateSize == 1) {
      if (Ad.getElement(1).subtract(1).isZero()) {
        DoubleMatrix A = Ad.createZero(1, 1);
        DoubleMatrix B = Bd.divide(samplingInterval);
        return new ArrayList<>(Arrays.asList(new DoubleMatrix[] {A, B}));
      }
    }

    final DoubleMatrix G = Matrix4.matrix4(Ad, Bd, Ad.createZero(inputSize, stateSize), Ad.createUnit(inputSize));
    final DoubleMatrix S = G.log().divide(samplingInterval);
    final DoubleMatrix A = S.getSubMatrix(1, stateSize, 1, stateSize);
    final DoubleMatrix B = S.getSubMatrix(1, stateSize, stateSize + 1, stateSize + inputSize);

    return new ArrayList<>(Arrays.asList(new DoubleMatrix[] {A, B}));
  }

}