Dric.java

/*
 * $Id: Dric.java,v 1.40 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.eig.DoubleEigenSolution;
import org.mklab.nfc.matrix.DoubleComplexMatrix;
import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.matrix.ElementHolder;
import org.mklab.nfc.matrix.IndexedMatrix;
import org.mklab.nfc.matrix.IntMatrix;
import org.mklab.nfc.scalar.DoubleNumber;
import org.mklab.tool.matrix.Matrix4;


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

  /**
   * @param A A行列
   * @param B B行列
   * @param Q Q行列
   * @param R R行列
   * @return 解 solution
   */
  public static DoubleMatrix dric(DoubleMatrix A, DoubleMatrix B, DoubleMatrix Q, DoubleMatrix R) {
    double tol1 = 1.0;
    double tol2 = 1.0E-6;
    return dric(A, B, Q, R, tol1, tol2);
  }

  /**
   * 離散時間システムのリカッティ方程式
   * 
   * <pre><code>
   * 
   * P - A#PA + A#PB(R + B'PB)&tilde; B'PA = Q </code></pre>
   * 
   * の安定化解を返します。解を求めるため、
   * 
   * <pre><code> [ A O] [u] = lambda [I BR'B'] [u] [-Q I] [v] [O A' ] [v] </code></pre>
   * 
   * を満たす安定な一般化固有ベクトルを用いる。
   * 
   * <p>リカッティ方程式の残差のフロベニウスノルムが<code>tol1</code>を大きいなら、 警告メッセージが表示されます。 デフォルトの<code>tol1</code>の値は、<code>1.0</code>です。
   * 
   * @param A A行列
   * @param B B行列
   * @param Q 状態の重み行列
   * @param R 入力の重み行列
   * @param tol1 方程式の残差の許容誤差
   * @return 解 solution
   */
  public static DoubleMatrix dric(DoubleMatrix A, DoubleMatrix B, DoubleMatrix Q, DoubleMatrix R, double tol1) {
    double tol2 = 1.0E-6;
    return dric(A, B, Q, R, tol1, tol2);
  }

  /**
   * もし、単位円上に閉ループ極があれば、警告メッセージが表示されます。 <code>tol2</code>で、単位円からの距離の許容誤差を指定できる デフォルトの<code>tol2</code>の値は、<code>1.0E-6</code>です。
   * 
   * @param A A行列
   * @param B B行列
   * @param Q 状態の重み行列
   * @param R 入力の重み行列
   * @param toleranceOfEquation 方程式の残差の許容誤差
   * @param toleranceOfPoles 安定極の判定の許容誤差
   * @return 解 solution
   */
  public static DoubleMatrix dric(DoubleMatrix A, DoubleMatrix B, DoubleMatrix Q, DoubleMatrix R, double toleranceOfEquation, double toleranceOfPoles) {
    int n = A.getRowSize();
    // int m = B.getColSize();

    DoubleMatrix HL = Matrix4.matrix4(A, A.createZero(n, n), Q.unaryMinus(), A.createUnit(n));
    DoubleMatrix HR = Matrix4.matrix4(A.createUnit(n), B.multiply(R.inverse()).multiply(B.conjugateTranspose()), A.createZero(n, n), A.conjugateTranspose());

    DoubleEigenSolution tmp = HL.eigenDecompose(HR);
    DoubleComplexMatrix EV = tmp.getValue();
    DoubleComplexMatrix T =  tmp.getVector();
    EV = EV.diagonalToVector();

    // Select finite eigenvalues and corresponding eigenvectors
    IntMatrix idx = EV.isFiniteElementWise().find();
    EV = EV.getSubMatrix(idx, 1);
    T = T.getColumnVectors(idx);

    // Sort eigenvalues by their absolute values
    IndexedMatrix<DoubleNumber,DoubleMatrix> tmp3 = EV.absElementWise().getRealPart().sort();
    //EV = tmp3.getMatrix();
    idx = tmp3.getIndices();

    DoubleComplexMatrix U = T.getSubMatrix(1, n, idx.getSubVector(1, n).transpose());
    DoubleComplexMatrix V = T.getSubMatrix(n + 1, 2 * n, idx.getSubVector(1, n).transpose());
    DoubleComplexMatrix Pc = V.multiply(U.inverse());

    DoubleMatrix P;
    P = Pc.getRealPart();
    P = P.transpose().add(P).divide(2);

    // Check redidual
    DoubleMatrix tmp1 = P.subtract(A.conjugateTranspose().multiply(P).multiply(A));
    DoubleMatrix tmp2 = A.conjugateTranspose().multiply(P).multiply(B).multiply(R.add(B.conjugateTranspose().multiply(P).multiply(B)).inverse()).multiply(B.conjugateTranspose().multiply(P).multiply(A));
    // DoubleMatrix Perr = P - A#*P*A + A#*P*B*(R + B#*P*B)~*B#*P*A - Q;
    DoubleMatrix Perr = tmp1.add(tmp2).subtract(Q);

    DoubleNumber residual = Perr.frobNorm();
    if (residual.isGreaterThan(toleranceOfEquation)) {
      System.err.println(Messages.getString("Dric.0") + residual); //$NON-NLS-1$
    }

    // Check unit-circle roots:
    DoubleMatrix F = R.add(B.conjugateTranspose().multiply(P).multiply(B)).leftDivide(B.conjugateTranspose().multiply(P).multiply(A));
    DoubleComplexMatrix pc = A.subtract(B.multiply(F)).eigenValue();
    ElementHolder<DoubleNumber> temp = ((pc.unaryMinus().addElementWise(1).absElementWise().getRealPart())).minimum();
    DoubleNumber tmp11 = temp.getElement();
    int row1 = temp.getRow();
    int column1 = temp.getColumn();

    if (tmp11.isLessThan(toleranceOfPoles)) {
      System.err.println(Messages.getString("Dric.1") + pc.getElement(row1, column1)); //$NON-NLS-1$
      System.err.println(Messages.getString("Dric.2")); //$NON-NLS-1$
    }

    // Check positive definiteness of P:
    DoubleComplexMatrix pc2 = P.eigenValue();
    ElementHolder<DoubleNumber> temp2 = pc2.getRealPart().minimum();
    DoubleNumber tmp22 = temp2.getElement();
    int row2 = temp2.getRow();
    int column2 = temp2.getColumn();

    if (tmp22.isLessThan(0)) {
      System.err.println(Messages.getString("Dric.3")); //$NON-NLS-1$
      System.err.println(Messages.getString("Dric.4") + pc2.getElement(row2, column2)); //$NON-NLS-1$
    }

    return P;
  }

}