Ctrm.java

/*
 * $Id: Ctrm.java,v 1.10 2008/02/28 00:42:50 koga Exp $
 *
 * Copyright (C) 2004 Koga Laboratory. All rights reserved.
 */
package org.mklab.tool.control;

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.RealNumericalScalar;


/**
 * 可制御性行列を求めるクラスです。
 * 
 * <p>Controllability matrix
 * 
 * @author koga
 * @version $Revision: 1.10 $
 * @see org.mklab.tool.control.Obsm
 * @see org.mklab.tool.control.Ctrf
 */
public class Ctrm {

  /**
   * 可制御性行列
   * 
   * <pre><code> V = [B AB A&circ;2B ...] </code></pre>
   * 
   * を返します。
   * 
   * @param A A行列
   * @param B B行列
   * @return (可制御性行列) (controllability matrix)
   */
  public static DoubleMatrix ctrm(DoubleMatrix A, DoubleMatrix B) {
    final String message;
    if (((message = Abcdchk.abcdchk(A, B)).length()) > 0) {
      throw new IllegalArgumentException(message);
    }

    final int stateSize = A.getRowSize();
    final DoubleMatrix AB = A.multiply(B);
    final DoubleMatrix V = AB.createZero(1, stateSize, AB);

    V.setSubMatrix(1, 1, B, B);
    for (int i = 1; i <= stateSize - 1; i++) {
      V.setSubMatrix(1, i + 1, B, A.multiply(V.getSubMatrix(1, i, B)));
    }
    return V;
  }

  /**
   * 可制御性行列
   * 
   * <pre><code> V = [B AB A&circ;2B ...] </code></pre>
   * 
   * を返します。
   * 
   * @param <RS> スカラーの型
   * @param <RM> 行列の型
   * @param <CS> 複素スカラーの型
   * @param <CM> 複素行列の型
   * @param A A行列
   * @param B B行列
   * @return (可制御性行列) (controllability 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>> RM ctrm(
      RM A, RM B) {
    final String message;
    if (((message = Abcdchk.abcdchk(A, B)).length()) > 0) {
      throw new IllegalArgumentException(message);
    }

    final int stateSize = A.getRowSize();
    final RM AB = A.multiply(B);
    final RM V = AB.createZero(1, stateSize, AB);

    V.setSubMatrix(1, 1, B, B);
    for (int i = 1; i <= stateSize - 1; i++) {
      V.setSubMatrix(1, i + 1, B, A.multiply(V.getSubMatrix(1, i, B)));
    }
    return V;
  }

}