Are.java

/*
 * $Id: Are.java,v 1.39 2008/07/17 07:30:03 koga Exp $
 * 
 * Copyright (C) 2004 Koga Laboratory. All rights reserved.
 */

package org.mklab.tool.control;

import java.util.List;

import org.mklab.nfc.eig.SchurDecomposition;
import org.mklab.nfc.matrix.ComplexNumericalMatrix;
import org.mklab.nfc.matrix.DoubleComplexMatrix;
import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.matrix.IntMatrix;
import org.mklab.nfc.matrix.RealNumericalMatrix;
import org.mklab.nfc.scalar.ComplexNumericalScalar;
import org.mklab.nfc.scalar.DoubleNumber;
import org.mklab.nfc.scalar.RealNumericalScalar;
import org.mklab.tool.matrix.Schord;


/**
 * 連続時間リカッティ方程式の解を求めるクラスです。
 * 
 * <p>Solution of continuous-time Riccati equation
 * 
 * @author koga
 * @version $Revision: 1.39 $
 * @see org.mklab.tool.control.Ric
 * @see org.mklab.tool.control.Lqr
 */
public class Are {

  /**
   * 連続時間リカッティ方程式
   * 
   * <pre><code> A#*P + P*A - P*R*P + Q = 0 </code></pre>
   * 
   * の解をシュア分解を用いて求め、その解を返します。
   * 
   * @param A システム行列 (system matrix)
   * @param R 入力に関する重み(非負定対称)行列 (symmetric and nonnegative definite weighting matrix for inputs)
   * @param Q 状態に関する重み(対称)行列 (symmetric weighting matrix for states)
   * @return リカッティ方程式の解 (solution)
   */
  public static DoubleMatrix are(DoubleMatrix A, DoubleMatrix R, DoubleMatrix Q) {
    return are(A, R, Q, -1);
  }

  /**
   * 虚軸に対する固有値の位置を判定するための許容誤差として<code>tolerance</code>を用いる。
   * 
   * @param A システム行列 (system matrix)
   * @param R 入力に関する重み(非負定対称)行列 (symmetric and nonnegative definite weighting matrix for inputs)
   * @param Q 状態に関する重み(対称)行列 (symmetric weighting matrix for states)
   * @param tolerance 許容誤差（tolerance)
   * @return リカッティ方程式の解 (solution)
   */
  public static DoubleMatrix are(DoubleMatrix A, DoubleMatrix R, DoubleMatrix Q, double tolerance) {
    return are(A, R, Q, new DoubleNumber(tolerance));
  }

  /**
   * 虚軸に対する固有値の位置を判定するための許容誤差として<code>tolerance</code>を用いる。
   * 
   * @param A システム行列 (system matrix)
   * @param R 入力に関する重み(非負定対称)行列 (symmetric and nonnegative definite weighting matrix for inputs)
   * @param Q 状態に関する重み(対称)行列 (symmetric weighting matrix for states)
   * @param tolerance 許容誤差（tolerance)
   * @return リカッティ方程式の解 (solution)
   */
  public static DoubleMatrix are(DoubleMatrix A, DoubleMatrix R, DoubleMatrix Q, DoubleNumber tolerance) {
    String message;
    if ((message = Abcdchk.abcdchk(A)).length() > 0) {
      throw new IllegalArgumentException(message);
    }

    int n = A.getRowSize();

    if (R.getRowSize() != n || R.getColumnSize() != n) {
      message = Messages.getString("Are.0") + R.getRowSize() + "x" + R.getColumnSize() + Messages.getString("Are.2"); //$NON-NLS-1$ //$NON-NLS-2$ //$NON-NLS-3$
      throw new IllegalArgumentException(message);
    }
    if (Q.getRowSize() != n || Q.getColumnSize() != n) {
      message = Messages.getString("Are.3") + Q.getRowSize() + "x" + Q.getColumnSize() + Messages.getString("Are.5"); //$NON-NLS-1$ //$NON-NLS-2$ //$NON-NLS-3$
      throw new IllegalArgumentException(message);
    }

    DoubleNumber unit = A.getElement(1, 1).createUnit();

    DoubleMatrix tmp1 = A.appendRight(R.unaryMinus());
    DoubleMatrix tmp2 = Q.unaryMinus().appendRight(A.conjugateTranspose().unaryMinus());
    DoubleMatrix tmp3 = tmp1.appendDown(tmp2);
    SchurDecomposition<DoubleNumber,DoubleMatrix> tmp4 = tmp3.schurDecompose();
    DoubleMatrix U = tmp4.getU();
    DoubleMatrix T = tmp4.getT();

    DoubleNumber localTolerance = tolerance.clone();

    if (localTolerance.isLessThan(0)) {
      localTolerance = (T.diagonalToVector().absElementWise()).max().multiply(10).multiply(unit.getMachineEpsilon());
    }

    /*
     * Location of eigenvalues: -1 : open left-half-plane 1 : open
     * right-half-plane 0 : within tolerance of imaginary axis
     */
    int np = 0;
    IntMatrix idx = new IntMatrix(1, 2 * n);
    for (int i = 1; i <= 2 * n; i++) {
      if (T.getElement(i, i).isLessThan(localTolerance.unaryMinus())) {
        idx.setElement(1, i, -1);
        np++;
      } else if (T.getElement(i, i).isGreaterThan(localTolerance)) {
        idx.setElement(1, i, 1);
      } else {
        idx.setElement(1, i, 0);
      }
    }

    if (np != n) {
      throw new IllegalArgumentException(Messages.getString("Are.6")); //$NON-NLS-1$
    }

    List<DoubleComplexMatrix> tmp = Schord.schord(U, T, idx);
    DoubleComplexMatrix Uc = tmp.get(0);

    DoubleMatrix P = Uc.getSubMatrix(n + 1, n + n, 1, n).divide(Uc.getSubMatrix(1, n, 1, n)).getRealPart();

    return P;
  }

  /**
   * 虚軸に対する固有値の位置を判定するための許容誤差として<code>tolerance</code>を用いる。
   * 
   * @param <RS> スカラーの型
   * @param <RM> 行列の型
   * @param <CS> 複素スカラーの型
   * @param <CM> 複素行列の型
   * @param A システム行列 (system matrix)
   * @param R 入力に関する重み(非負定対称)行列 (symmetric and nonnegative definite weighting matrix for inputs)
   * @param Q 状態に関する重み(対称)行列 (symmetric weighting matrix for states)
   * @param tolerance 許容誤差（tolerance)
   * @return リカッティ方程式の解 (solution)
   */
  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 are(RM A, RM R, RM Q, RS tolerance) {
   String message;
    if ((message = Abcdchk.abcdchk(A)).length() > 0) {
      throw new IllegalArgumentException(message);
    }

    int n = A.getRowSize();

    if (R.getRowSize() != n || R.getColumnSize() != n) {
      message = Messages.getString("Are.0") + R.getRowSize() + "x" + R.getColumnSize() + Messages.getString("Are.2"); //$NON-NLS-1$ //$NON-NLS-2$ //$NON-NLS-3$
      throw new IllegalArgumentException(message);
    }
    if (Q.getRowSize() != n || Q.getColumnSize() != n) {
      message = Messages.getString("Are.3") + Q.getRowSize() + "x" + Q.getColumnSize() + Messages.getString("Are.5"); //$NON-NLS-1$ //$NON-NLS-2$ //$NON-NLS-3$
      throw new IllegalArgumentException(message);
    }

    RS unit = A.getElement(1, 1).createUnit();

    RM tmp1 = A.appendRight(R.unaryMinus());
    RM tmp2 = Q.unaryMinus().appendRight(A.conjugateTranspose().unaryMinus());
    RM tmp3 = tmp1.appendDown(tmp2);
    SchurDecomposition<RS,RM> tmp4 = tmp3.schurDecompose();
    RM U = tmp4.getU();
    RM T = tmp4.getT();

    RS localTolerance = tolerance.clone();

    if (localTolerance.isLessThan(0)) {
      localTolerance = (T.diagonalToVector().absElementWise()).max().multiply(10).multiply(unit.getMachineEpsilon());
    }

    /*
     * Location of eigenvalues: -1 : open left-half-plane 1 : open
     * right-half-plane 0 : within tolerance of imaginary axis
     */
    int np = 0;
    IntMatrix idx = new IntMatrix(1, 2 * n);
    for (int i = 1; i <= 2 * n; i++) {
      if (T.getElement(i, i).isLessThan(localTolerance.unaryMinus())) {
        idx.setElement(1, i, -1);
        np++;
      } else if (T.getElement(i, i).isGreaterThan(localTolerance)) {
        idx.setElement(1, i, 1);
      } else {
        idx.setElement(1, i, 0);
      }
    }

    if (np != n) {
      throw new IllegalArgumentException(Messages.getString("Are.6")); //$NON-NLS-1$
    }

    List<CM> tmp = Schord.schord(U, T, idx);
    CM Uc = tmp.get(0);

    RM P = Uc.getSubMatrix(n + 1, n + n, 1, n).divide(Uc.getSubMatrix(1, n, 1, n)).getRealPart();

    return P;
  }

}