Lqry.java

/*
 * $Id: Lqry.java,v 1.10 2008/03/23 14:29:08 koga Exp $
 *
 * Copyright (C) 2004 Koga Laboratory. All rights reserved.
 */
package org.mklab.tool.control;

import java.util.List;

import org.mklab.nfc.matrix.DoubleMatrix;


/**
 * 連続時間システムの出力重み付きLQRを求めるクラスです。
 * 
 * <p>Continuous-time LQR with output weighting
 * 
 * @author koga
 * @version $Revision: 1.10 $
 * @see org.mklab.tool.control.Lqr
 * @see org.mklab.tool.control.Lqrs
 */
public class Lqry {

  /**
   * 連続時間線形システム
   * 
   * <pre><code> . x = Ax + Bu y = Cx + Du </code></pre>
   * 
   * について、二次形式評価関数
   * 
   * <pre><code> J = Integral (y#Qy + u#Ru) dt </code></pre>
   * 
   * を最小にする最適フィードバック則<code>u = -Fy</code>の フィードバックゲイン行列<code>F</code>を返します。
   * 
   * <p> また、リカッティ方程式
   * 
   * <pre><code> P A + A# P - P B R&tilde; B# P + Q = 0 </code></pre>
   * 
   * の解<code>P</code>を返します。
   * 
   * @param A A行列
   * @param B B行列
   * @param C C行列
   * @param D D行列
   * @param Q 状態に関する重み行列
   * @param R 入力に関する重み行列
   * @return {F,P} (状態フィードバックゲイン, リカッティ方程式の解) regulator
   */
  public static List<DoubleMatrix> lqry(DoubleMatrix A, DoubleMatrix B, DoubleMatrix C, DoubleMatrix D, DoubleMatrix Q, DoubleMatrix R) {
    String message;
    if ((message = Abcdchk.abcdchk(A, B, C, D)).length() > 0) {
      throw new RuntimeException(message);
    }

    return Lqr.lqr(A, B, C.conjugateTranspose().multiply(Q).multiply(C), R.add(D.conjugateTranspose().multiply(Q).multiply(D)), C.conjugateTranspose().multiply(Q).multiply(D));
  }

  /**
   * <code>S</code>を<code>u</code>と<code>y</code>とのクロス項を指定するために使用します。
   * 
   * <pre><code> J = Integral (y#Qy + u#Ru + 2 y#Su) dt </code></pre>
   * 
   * @param A A行列
   * @param B B行列
   * @param C C行列
   * @param D D行列
   * @param Q 状態に関する重み行列
   * @param R 入力に関する重み行列
   * @param S 入力と出力に関する重み行列
   * @return {F,P} (状態フィードバックゲイン, リカッティ方程式の解) regulator
   */
  public static List<DoubleMatrix> lqry(DoubleMatrix A, DoubleMatrix B, DoubleMatrix C, DoubleMatrix D, DoubleMatrix Q, DoubleMatrix R, DoubleMatrix S) {
    String message;
    if ((message = Abcdchk.abcdchk(A, B, C, D)).length() > 0) {
      throw new RuntimeException(message);
    }

    return Lqr.lqr(A, B, C.conjugateTranspose().multiply(Q).multiply(C),
        R.add(D.conjugateTranspose().multiply(S)).add(S.conjugateTranspose().multiply(D)).add(D.conjugateTranspose().multiply(Q).multiply(D)),
        C.conjugateTranspose().multiply(Q).multiply(D).add(C.conjugateTranspose().multiply(S)));
  }

}