Ric.java

/*
 * $Id: Ric.java,v 1.34 2008/07/17 07:30:03 koga Exp $
 *
 * Copyright (C) 2004 Koga Laboratory. All rights reserved.
 */
package org.mklab.tool.control;

import org.mklab.nfc.matrix.DoubleComplexMatrix;
import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.matrix.ElementHolder;
import org.mklab.nfc.scalar.DoubleNumber;


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

  /**
   * リカッティ方程式の式誤差のフロベニウスノルムが<code>residulaTolerance</code>より大きいとき、 警告メッセージが表示されます。
   * 
   * @param A A行列
   * @param Q 状態に関する重み行列
   * @param R 入力に関する重み行列
   * @param residualTolerance リカッティ方程式の残差の許容誤差
   * @return 解 solution
   */
  public static DoubleMatrix ric(final DoubleMatrix A, final DoubleMatrix Q, final DoubleMatrix R, final double residualTolerance) {
    final double poleTolerance = 1.0E-6;
    return ric(A, Q, R, residualTolerance, poleTolerance);
  }

  /**
   * 連続時間のリカッティ方程式
   * 
   * <pre><code> A#*P + P*A - P*R*P + Q = 0 </code></pre>
   * 
   * の安定化解を有本・ポッターの方法で求めます。
   * 
   * @param A システム行列
   * @param Q 状態に関する重み行列(対称)
   * @param R 入力に関する重み行列(対称、準正定)
   * @return 解 (solution)
   */
  public static DoubleMatrix ric(final DoubleMatrix A, final DoubleMatrix Q, final DoubleMatrix R) {
    final double residualTolerance = 1.0;
    final double poleTolerance = 1.0E-6;
    return ric(A, Q, R, residualTolerance, poleTolerance);
  }

  /**
   * もし、リカッティ方程式の式誤差のフロベニウスノルムが<code>residulaTolerance</code>より 大きいなら、警告メッセージが表示されます。
   * 
   * <p> もし、jw軸(虚軸)上に閉ループ系の極があれば、警告メッセージが表示されます。 jw軸(虚軸)からの距離が<code>poleTolerance</code>より小さい閉ループ系の極があれば、 警告メッセージが表示されます。
   * 
   * @param A システム行列
   * @param Q 状態に関する重み行列
   * @param R 入力に関する重み行列
   * @param residualTolerance リカッティ方程式の残差の許容誤差
   * @param poleTolerance 安定極の判定許容誤差
   * @return 解 solution
   */
  public static DoubleMatrix ric(DoubleMatrix A, DoubleMatrix Q, DoubleMatrix R, double residualTolerance, double poleTolerance) {
    DoubleMatrix H = A.appendRight(R.unaryMinus()).appendDown(Q.unaryMinus().appendRight(A.conjugateTranspose().unaryMinus()));

    DoubleComplexMatrix T = H.eigenVector();
    DoubleComplexMatrix U = T.getSubMatrix(1, 2, A);
    DoubleComplexMatrix V = T.getSubMatrix(2, 2, A);

    DoubleComplexMatrix Pc = V.multiply(U.inverse());
    DoubleMatrix P;

    if (A.isReal() && Q.isReal() && R.isReal()) {
      P = Pc.getRealPart();
      P = Pc.transpose().add(Pc).divide(2).getRealPart();
    } else {
      P = Pc.conjugateTranspose().add(Pc).divide(2).getRealPart();
    }

    // Check redidual
    DoubleMatrix Perr = A.conjugateTranspose().multiply(P).add(P.multiply(A)).subtract(P.multiply(R).multiply(P)).add(Q);
    DoubleNumber res = Perr.frobNorm();
    if (res.isGreaterThan(residualTolerance)) {
      System.err.println(Messages.getString("Ric.0") + res); //$NON-NLS-1$
    }

    // Check jw-axis roots:
    DoubleComplexMatrix pc1 = A.subtract(R.multiply(P)).eigenValue();
    ElementHolder<DoubleNumber> tmp1 = (pc1.getRealPart().absElementWise()).minimum();
    DoubleNumber minimumResidue = tmp1.getElement();
    int row1 = tmp1.getRow();
    int column1 = tmp1.getColumn();
    if (minimumResidue.isLessThan(poleTolerance)) {
      System.err.println(Messages.getString("Ric.1") + pc1.getElement(row1, column1)); //$NON-NLS-1$
      System.err.println(Messages.getString("Ric.2")); //$NON-NLS-1$
    }

    // Check positive definiteness of P:
    DoubleComplexMatrix pc2 = P.eigenValue();
    ElementHolder<DoubleNumber> tmp2 = pc2.getRealPart().minimum();
    DoubleNumber minimumPoleError = tmp2.getElement();
    int row2 = tmp2.getRow();
    int column2 = tmp2.getColumn();
    if (minimumPoleError.isLessThan(0)) {
      System.err.println(Messages.getString("Ric.3")); //$NON-NLS-1$
      System.err.println(Messages.getString("Ric.4") + pc2.getElement(row2, column2)); //$NON-NLS-1$
    }

    return P;
  }

}