Gramian.java

/*
 * $Id: Gramian.java,v 1.11 2008/02/02 15:08:43 koga Exp $
 *
 * Copyright (C) 2004 Koga Laboratory. All rights reserved.
 */
package org.mklab.tool.control;

import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.scalar.DoubleNumber;
import org.mklab.nfc.svd.SingularValueDecomposition;


/**
 * 連続系の可制御性グラミアンと可観測性グラミアンを求めるクラスです。
 * 
 * <p>Continuous-time controllability and observability gramian
 * 
 * @author koga
 * @version $Revision: 1.11 $
 * @see org.mklab.tool.control.Dgramian
 * @see org.mklab.tool.control.Ctrm
 * @see org.mklab.tool.control.Obsm
 */
public class Gramian {

  /**
   * <code>gramian(A,B)</code>は、連続時間システムの可制御性グラミアンを返します。
   * 
   * <p><code>gramian(A#,C#)</code>は、連続時間システムの可観測性グラミアンを返します。
   * 
   * @param A A行列
   * @param B B行列
   * @return グラミアン (gramian)
   */
  public static DoubleMatrix gramian(DoubleMatrix A, DoubleMatrix B) {
    String message;
    if ((message = Abcdchk.abcdchk(A, B)).length() > 0) {
      throw new RuntimeException(message);
    }

    SingularValueDecomposition<DoubleNumber,DoubleMatrix> tmp = B.singularValueDecompose();
    DoubleMatrix U = tmp.getU();
    DoubleMatrix D = tmp.getD();
    // DoubleMatrix V = tmp[2];
    DoubleMatrix P = Lyap.lyap(U.conjugateTranspose().multiply(A).multiply(U), D.multiply(D.conjugateTranspose()));
    DoubleMatrix G = U.multiply(P).multiply(U.conjugateTranspose());
    G = G.add(G.conjugateTranspose()).divide(2);
    return G;
  }

}