DoubleLqrDesigner.java

/*
 * $Id: LqrDesigner.java,v 1.8 2008/07/16 04:58:04 koga Exp $
 *
 * Copyright (C) 2004 Koga Laboratory. All rights reserved.
 *
 */
package org.mklab.tool.control.system.controller;

import org.mklab.nfc.matrix.DoubleComplexMatrix;
import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.tool.control.Lqr;
import org.mklab.tool.control.system.DoubleLinearSystemOperator;
import org.mklab.tool.control.system.DoubleSystemOperator;


/**
 * LQ最適制御(安定化)のための状態フィードバックを設計するクラスです。
 * 
 * @author koga
 * @version $Revision: 1.8 $, 2004/05/31
 */
public class DoubleLqrDesigner {

  /** 制御対象(線形システム) */
  private DoubleLinearSystemOperator plant;
  /** 状態に関する重み行列 */
  private DoubleMatrix Q;
  /** 入力に関する重み行列 */
  private DoubleMatrix R;
  /** LQ最適制御のための状態フィードバックゲイン行列 */
  private DoubleMatrix F;

  /**
   * コンストラクター
   * 
   * @param plant 制御対象(線形システム)
   */
  public DoubleLqrDesigner(final DoubleSystemOperator plant) {
    this.plant = (DoubleLinearSystemOperator)plant;
  }

  /**
   * LQ最適制御のための状態フィードバック行列を返します。
   * 
   * @return LQ最適制御のための状態フィードバック行列
   */
  public DoubleMatrix getStateFeedback() {
    design();
    return this.F;
  }

  /**
   * LQ最適制御のための状態フィードバック行列を設計します。
   */
  private void design() {
    this.F = Lqr.lqr(this.plant.getA(), this.plant.getB(), this.Q, this.R).get(0);
  }

  /**
   * 重み行列QとRを設定します。
   * 
   * @param Q 状態に関する重み行列
   * @param R 入力に関する重み行列
   */
  public void setWeightingMatrices(final DoubleMatrix Q, final DoubleMatrix R) {
    this.Q = Q;
    this.R = R;
  }

  /**
   * 閉ループ系の極を表示します。
   */
  public void showClosedLoopPoles() {
    design();
    final DoubleMatrix A = this.plant.getA();
    final DoubleMatrix B = this.plant.getB();
    final DoubleComplexMatrix closedLoopPoles = A.subtract(B.multiply(this.F)).eigenValue();
    closedLoopPoles.print(Messages.getString("LqrDesigner.0")); //$NON-NLS-1$
  }

  //  /**
  //   * メインメソッド
  //   * 
  //   * @param args
  //   *        コマンドライン引数
  //   */
  //  public static void main(String[] args) {
  //    final LqrDesigner designer = new LqrDesigner(new LinearSinglePendulum());
  //    final DoubleMatrix Q = DoubleMatrix.diagonal(new double[] {1.0E5, 1.0E5, 1, 1});
  //    final DoubleMatrix R = new DoubleMatrix(new double[] {1});
  //    designer.setWeightingMatrix(Q, R);
  //    System.out.println("LQ最適制御のための状態フィードバック行列");
  //    designer.getStateFeedback().print("F");
  //    System.out.println("");
  //    designer.showClosedLoopPoles();
  //  }
}