Lqrs.java

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

import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;

import org.mklab.nfc.matrix.DoubleMatrix;


/**
 * 連続時間システムのLQRをシュアーアルゴリズムを用いて求めるクラスです。
 * 
 * <p>Continuous-time linear quadratic regulator (Schur algorithm)
 * 
 * @author koga
 * @version $Revision: 1.14 $
 * @see org.mklab.tool.control.Lqr
 * @see org.mklab.tool.control.Lqry
 * @see org.mklab.tool.control.Lqe
 */
public class Lqrs {

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

    DoubleMatrix Ri = R.inverse();
    DoubleMatrix P = Are.are(A, B.multiply(Ri).multiply(B.conjugateTranspose()), Q);
    DoubleMatrix F = Ri.multiply(B.conjugateTranspose()).multiply(P);

    return new ArrayList<>(Arrays.asList(new DoubleMatrix[] {F, P}));
    //return new MatxList(new Object[] {F, P});
  }

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

    DoubleMatrix Ri = R.inverse();
    final DoubleMatrix a = A.subtract(B.multiply(Ri).multiply(S.conjugateTranspose()));
    final DoubleMatrix r = B.multiply(Ri).multiply(B.conjugateTranspose());
    final DoubleMatrix q = Q.subtract(S.multiply(Ri).multiply(S.conjugateTranspose()));
    DoubleMatrix P = Are.are(a, r, q);
    DoubleMatrix F = Ri.multiply(S.conjugateTranspose().add(B.conjugateTranspose().multiply(P)));

    return new ArrayList<>(Arrays.asList(new DoubleMatrix[] {F, P}));
    //return new MatxList(new Object[] {F, P});
  }
}