DoublePoleViewer.java

/*
 * $Id: PoleViewer.java,v 1.14 2008/03/30 00:30:47 koga Exp $
 *
 * Copyright (C) 2004 Koga Laboratory. All rights reserved.
 *
 */
package org.mklab.tool.control.system.controller;

import java.io.IOException;

import org.mklab.nfc.matrix.DoubleComplexMatrix;
import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.util.Pause;
import org.mklab.tool.control.system.DoubleLinearSystemOperator;
import org.mklab.tool.graph.gnuplot.Canvas;
import org.mklab.tool.graph.gnuplot.Gnuplot;


/**
 * 閉ループ系の極とオブザーバの極を複素平面にプロットするクラスです。
 * 
 * @author koga
 * @version $Revision: 1.14 $, 2004/05/31
 */
public class DoublePoleViewer {

  /** 最大値の値と描画領域の比率 */
  private static final double RATIO_OF_VIEWREGION_MAXVALUE = 1.5;

  /** 実部が負の領域と正の領域の比率 */
  private static final double RATIO_OF_NEGATIVE_POSITIVE_REGION = 1.0 / 4.0;

  /** 倒立振子の線形モデル */
  private DoubleLinearSystemOperator linearModel;

  /** 描画範囲のxの最大値 */
  private double xMax;

  /** 描画範囲のxの最小値 */
  private double xMin;

  /** 描画範囲のyの最大値 */
  private double yMax;

  /** 描画範囲のyの最小値 */
  private double yMin;

  /**
   * コンストラクタ
   * 
   * @param linearModel 倒立振子の線形モデル
   */
  public DoublePoleViewer(DoubleLinearSystemOperator linearModel) {
    this.linearModel = linearModel;
  }

  /**
   * 閉ループ系の極とオブザーバの極を複素平面にプロットします。
   * 
   * @param F 状態フィードバック行列
   * @param observer オブザーバ
   * @throws IOException キーボードから入力できない場合
   */
  public void plot(DoubleMatrix F, DoubleLinearSystemOperator observer) throws IOException {
    DoubleMatrix A = this.linearModel.getA();
    DoubleMatrix B = this.linearModel.getB();
    DoubleMatrix Ah = observer.getA();
    DoubleComplexMatrix closedLoopPoles = A.subtract(B.multiply(F)).eigenValue();
    DoubleComplexMatrix observerPoles = Ah.eigenValue();

    setupRange(closedLoopPoles, observerPoles);
    plotPoles(closedLoopPoles, observerPoles);
  }

  /**
   * 閉ループ系の極とオブザーバの極をプロットします。
   * 
   * @param closedLoopPoles 閉ループ系の極
   * @param observerPoles オブザーバの極
   * @throws IOException キーボードから入力できない場合
   */
  @SuppressWarnings("nls")
  private void plotPoles(DoubleComplexMatrix closedLoopPoles, DoubleComplexMatrix observerPoles) throws IOException {
    Gnuplot gp = new Gnuplot();
    Canvas canvas = gp.createCanvas();
    canvas.doCommand("set data style points");
    canvas.setXLabel("Re");
    canvas.setYLabel("Im");
    canvas.setGridVisible(true);
    canvas.setTitle("Closed-loop poles and observer poles");
    canvas.doCommand("set xrange [" + RATIO_OF_VIEWREGION_MAXVALUE * this.xMin + ":" + RATIO_OF_VIEWREGION_MAXVALUE * this.xMax + "]");
    canvas.doCommand("set yrange [" + RATIO_OF_VIEWREGION_MAXVALUE * this.yMin + ":" + RATIO_OF_VIEWREGION_MAXVALUE * this.yMax + "]");

    canvas.plot(closedLoopPoles.getRealPart().transpose(), closedLoopPoles.getImaginaryPart().transpose(), new String[] {"closed-loop poles"});

    canvas.setHolding(true);
    canvas.plot(observerPoles.getRealPart().transpose(), observerPoles.getImaginaryPart().transpose(), new String[] {"observer poles"});
    canvas.setHolding(false);

    Pause.pause();
    gp.close();
  }

  /**
   * 閉ループ系の極とオブザーバーの極の複素平面上での位置を考慮して描画範囲を設定します。
   * 
   * @param closedLoopPoles 閉ループ系の極
   * @param observerPoles オブザーバの極
   */
  private void setupRange(DoubleComplexMatrix closedLoopPoles, DoubleComplexMatrix observerPoles) {
    DoubleMatrix realPart = closedLoopPoles.getRealPart().appendDown(observerPoles.getRealPart());
    DoubleMatrix imagPart = closedLoopPoles.getImaginaryPart().appendDown(observerPoles.getImaginaryPart());
    this.xMax = realPart.max().doubleValue();
    this.xMin = realPart.min().doubleValue();
    this.yMax = imagPart.max().doubleValue();
    this.yMin = imagPart.min().doubleValue();
    if (this.xMax < 0.0) {
      this.xMax = -this.xMin * RATIO_OF_NEGATIVE_POSITIVE_REGION;
    }
    if (this.xMin > 0.0) {
      this.xMin = -this.xMax * RATIO_OF_NEGATIVE_POSITIVE_REGION;
    }
  }
  //
  //  /**
  //   * メインメソッド
  //   * 
  //   * @param args
  //   *        コマンドライン引数
  //   * @throws InterruptedException
  //   *         キャンセルボタンが押された場合
  //   * @throws IOException キーボードから入力できない場合 
  //   */
  //  public static void main(String[] args) throws InterruptedException, IOException {
  //    final LinearSinglePendulum linearPendulum = new LinearSinglePendulum();
  //    final DoubleMatrix Q = new DoubleMatrix(new double[] {1.0E5, 1.0E5, 1, 1}).vectorToDiagonal();
  //    final DoubleMatrix R = new DoubleMatrix(new double[] {1});
  //    final LqrStateFeedback stateFeedback = new LqrStateFeedback();
  //    stateFeedback.setWeightingMatrix(Q, R);
  //    final DoubleMatrix F = stateFeedback.getGain();
  //    
  //    final DoubleMatrix observerPoles = new DoubleComplexMatrix(new double[] {-20, -20}, new double[] {0, 0}).transpose();
  //    final ContinuousObserver observer = new ContinuousObserver();
  //    observer.setObserverPoles(observerPoles);
  //    
  //    final PoleViewer viewer = new PoleViewer(linearPendulum);
  //    viewer.plot(F, observer);
  //  }

}