LqrDesigner.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.ComplexNumericalMatrix;
import org.mklab.nfc.matrix.RealNumericalMatrix;
import org.mklab.nfc.scalar.ComplexNumericalScalar;
import org.mklab.nfc.scalar.RealNumericalScalar;
import org.mklab.tool.control.Lqr;
import org.mklab.tool.control.system.LinearSystemOperator;


/**
 * LQ最適制御(安定化)のための状態フィードバックを設計するクラスです。
 * 
 * @author koga
 * @version $Revision: 1.8 $, 2004/05/31
  * @param <RS> type of real scalar
 * @param <RM> type of real matrix
 * @param <CS> type of complex scalar
 * @param <CM> type of complex matrix
*/
public class LqrDesigner<RS extends RealNumericalScalar<RS, RM, CS, CM>, RM extends RealNumericalMatrix<RS, RM, CS, CM>, CS extends ComplexNumericalScalar<RS, RM, CS, CM>, CM extends ComplexNumericalMatrix<RS, RM, CS, CM>> {

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

  /**
   * コンストラクター
   * 
   * @param plant 制御対象(線形システム)
   */
  public LqrDesigner(final LinearSystemOperator<RS,RM,CS,CM> plant) {
    this.plant = plant;
  }

  /**
   * LQ最適制御のための状態フィードバック行列を返します。
   * 
   * @return LQ最適制御のための状態フィードバック行列
   */
  public RM 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 RM Q, final RM R) {
    this.Q = Q;
    this.R = R;
  }

  /**
   * 閉ループ系の極を表示します。
   */
  public void showClosedLoopPoles() {
    design();
    final RM A = this.plant.getA();
    final RM B = this.plant.getB();
    final CM 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 RM Q = DoubleMatrix.diagonal(new double[] {1.0E5, 1.0E5, 1, 1});
  //    final RM R = new DoubleMatrix(new double[] {1});
  //    designer.setWeightingMatrix(Q, R);
  //    System.out.println("LQ最適制御のための状態フィードバック行列");
  //    designer.getStateFeedback().print("F");
  //    System.out.println("");
  //    designer.showClosedLoopPoles();
  //  }
}