ObserverDesigner.java

/*
 * $Id: ObserverDesigner.java,v 1.5 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.util.List;

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.Obsg;
import org.mklab.tool.control.system.LinearSystemOperator;
import org.mklab.tool.control.system.continuous.ContinuousLinearDynamicSystem;


/**
 * ゴピナスの方法で連続時間最小次元オブザーバを設計するクラスです。
 * 
 * @author koga
 * @version $Revision: 1.5 $, 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 ObserverDesigner<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 CM observerPoles;
  /** オブザーバー */
  private ContinuousLinearDynamicSystem<RS,RM,CS,CM> observer;

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

  /**
   * オブザーバーの極を設定します。
   * 
   * @param observerPoles オブザーバーの極
   */
  public void setObserverPoles(final CM observerPoles) {
    this.observerPoles = observerPoles;
  }

  /**
   * 最小次元連続時間オブザーバを返します。
   * 
   * @return 連続時間オブザーバ
   */
  public ContinuousLinearDynamicSystem<RS,RM,CS,CM> getObserver() {
    design();
    return this.observer;
  }

  /**
   * ゴピナスの方法で最小次元連続時間オブザーバを設計します。
   */
  private void design() {
    RS sunit = this.plant.getA().getElement(1,1).createUnit();

    final List<RM> coefficientMatrices = Obsg.obsg(this.plant.getA(), this.plant.getB(), this.plant.getC(), this.observerPoles);

    final RM Ah = coefficientMatrices.get(0);
    final RM Bh = coefficientMatrices.get(1);
    final RM Ch = coefficientMatrices.get(2);
    final RM Dh = coefficientMatrices.get(3);
    final RM Jh = coefficientMatrices.get(4);

    final RM A = Ah;
    final RM B = Jh.appendRight(Bh);
    final RM C = Ch;
    final RM D =  sunit.createZeroGrid(Dh.getRowSize(), Jh.getColumnSize()).appendRight(Dh);
    this.observer = new ContinuousLinearDynamicSystem<>(A, B, C, D, sunit);
  }

  /**
   * 状態推定の対象(システム)の入力数を返します。
   * 
   * @return 状態推定の対象(システム)の入力数
   */
  public int getPlantInputSize() {
    return this.plant.getB().getColumnSize();
  }

  /**
   * 状態推定の対象(システム)の出力数を返します。
   * 
   * @return 状態推定の対象(システム)の出力数
   */
  public int getPlantOutputSize() {
    return this.plant.getC().getRowSize();
  }

  /**
   * オブザーバの係数行列を表示します。
   */
  @SuppressWarnings("nls")
  public void showObserver() {
    design();

    final int inputSize = getPlantInputSize();
    final int outputSize = getPlantOutputSize();

    final RM Ah = this.observer.getA();
    final RM Jh = this.observer.getB().getColumnVectors(1, inputSize);
    final RM Bh = this.observer.getB().getColumnVectors(inputSize + 1, inputSize + outputSize);
    final RM Ch = this.observer.getC();
    final RM Dh = this.observer.getD().getColumnVectors(inputSize + 1, inputSize + outputSize);

    Ah.print("Ah");
    System.out.println("");
    Jh.print("Jh");
    System.out.println("");
    Bh.print("Bh");
    System.out.println("");
    Ch.print("Ch");
    System.out.println("");
    Dh.print("Dh");
    System.out.println("");
  }

  /**
   * オブザーバーの極が指定済みで、オブザーバーが設計可能であるか判定します。
   * 
   * @return オブザーバーが設計可能ならばtrue、そうでなければfalse
   */
  public boolean isAvailable() {
    if (this.observerPoles == null) {
      return false;
    }
    return true;
  }

  //
  //  /**
  //   * メインメソッド
  //   * 
  //   * @param args
  //   *        コマンドライン引数
  //   */
  //  public static void main(String[] args) {
  //    final ObserverDesigner designer = new ObserverDesigner(new LinearSinglePendulum());
  //    final RM observerPoles = new DoubleComplexMatrix(new double[] {-20, -20}, new double[] {0, 0}).transpose();
  //    designer.setObserverPoles(observerPoles);
  //    designer.showObserver();
  //  }

  /**
   * 状態推定の対象(線形システム)を設定します。
   * 
   * @param plant 状態推定の対象(線形システム)
   */
  public void setPlant(final LinearSystemOperator<RS,RM,CS,CM> plant) {
    this.plant = plant;
  }
}