C2d.java

/*
 * $Id: C2d.java,v 1.20 2008/03/23 14:29:08 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.ComplexNumericalMatrix;
import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.matrix.RealNumericalMatrix;
import org.mklab.nfc.scalar.ComplexNumericalScalar;
import org.mklab.nfc.scalar.DoubleNumber;
import org.mklab.nfc.scalar.RealNumericalScalar;


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

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

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

    final int stateSize = A.getRowSize();
    final int inputSize = B.getColumnSize();

    final DoubleMatrix S = (A.appendRight(B).appendDown(A.createZero(inputSize, inputSize + stateSize)).multiply(samplingInterval)).exp();
    final DoubleMatrix Ad = S.getSubMatrix(1, stateSize, 1, stateSize);
    final DoubleMatrix Bd = S.getSubMatrix(1, stateSize, stateSize + 1, stateSize + inputSize);

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

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

  /**
   * 連続時間システム
   * 
   * <pre><code> dx/dt = Ax + Bu </code></pre>
   * 
   * を離散時間システム
   * 
   * <pre><code> x[n + 1] = Ad * x[n] + Bd * u[n] </code></pre>
   * 
   * に変換します。ただし、入力はゼロ次ホールドを通して与えられるとします。
   * 
   * @param A 連続時間系のシステム行列(system matrix)
   * @param B 連続時間系の入力行列 (input matrix)
   * @param samplingInterval サンプリング周期 (sampling interval)
   * @return {Ab, Bd}(離散化システム) (discrete-time system)
   * @param <RS> type of real scalar
   * @param <RM> type of real matrix
   * @param <CS> type of complex scalar
   * @param <CM> type of complex matrix
   */
  public static <RS extends RealNumericalScalar<RS, RM, CS, CM>, RM extends RealNumericalMatrix<RS, RM, CS, CM>, CS extends ComplexNumericalScalar<RS, RM, CS, CM>, CM extends ComplexNumericalMatrix<RS, RM, CS, CM>> List<RM> c2d(
      RM A, RM B, RS samplingInterval) {
    final String message;
    if ((message = Abcdchk.abcdchk(A, B)).length() > 0) {
      throw new IllegalArgumentException(message);
    }

    final int stateSize = A.getRowSize();
    final int inputSize = B.getColumnSize();

    final RM S = (A.appendRight(B).appendDown(A.createZero(inputSize, inputSize + stateSize)).multiply(samplingInterval)).exp();
    final RM Ad = S.getSubMatrix(1, stateSize, 1, stateSize);
    final RM Bd = S.getSubMatrix(1, stateSize, stateSize + 1, stateSize + inputSize);

    List<RM> ab = new ArrayList<>();
    ab.add(Ad);
    ab.add(Bd);
    return ab;
  }
}