MotorContinuousObserver.java

/*
 * Created on 2011/07/20
 * Copyright (C) 2011 Koga Laboratory. All rights reserved.
 *
 */
package org.mklab.tool.control.model.matxbook.composite;


import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.tool.control.model.matxbook.ContinuousObserver;
import org.mklab.tool.control.model.matxbook.Motor;
import org.mklab.tool.control.model.matxbook.StateFeedback;
import org.mklab.tool.control.system.DoubleSystemBuilder;
import org.mklab.tool.control.system.DoubleSystemOperator;
import org.mklab.tool.control.system.math.DoubleConstantSystem;
import org.mklab.tool.control.system.source.DoubleConstantSource;


/**
 * モータと連続時間オブザーバの結合システムを表すクラスです。
 * 
 * @author koga
 * @version $Revision$, 2011/07/20
 */
public class MotorContinuousObserver {

  private DoubleSystemBuilder reference;
  private DoubleSystemBuilder motor;
  private DoubleSystemBuilder stateFeedback;
  private DoubleSystemBuilder observer;
  private DoubleSystemBuilder m1;
  private DoubleSystemBuilder m2;

  /**
   * モータと連続時間オブザーバの結合システムを返します。
   * 
   * @return モータと連続時間オブザーバの結合システム
   */
  public DoubleSystemOperator getSystem() {
    createPlant();
    createController();
    createAuxiliary();

    DoubleSystemBuilder inputOutput = this.m1.add(this.m2.multiply(this.motor));
    DoubleSystemBuilder forward = inputOutput.multiply(this.stateFeedback);
    DoubleSystemBuilder closedLoop = forward.feedback(this.observer);
    DoubleSystemBuilder all = closedLoop.multiply(this.reference);
    return all.getSystemOperator();
  }

  private void createPlant() {
    // モーター
    Motor motorSystem = new Motor();
    DoubleMatrix initialState = new DoubleMatrix(new double[][] { {1}, {0}});
    motorSystem.setInitialState(initialState);
    this.motor = new DoubleSystemBuilder(motorSystem);
  }

  private void createController() {
    // 状態フィードバック
    StateFeedback stateFeedbackSystem = new StateFeedback();
    this.stateFeedback = new DoubleSystemBuilder(stateFeedbackSystem);

    // 連続時間状態観測器
    ContinuousObserver observerSystem = new ContinuousObserver();
    observerSystem.setInitialState(new DoubleMatrix(1, 1));
    this.observer = new DoubleSystemBuilder(observerSystem);
  }

  private void createAuxiliary() {
    // 零入力
    DoubleMatrix zero = new DoubleMatrix(2, 1);
    DoubleConstantSource referenceSystem = new DoubleConstantSource(zero);
    this.reference = new DoubleSystemBuilder(referenceSystem);

    // k1, k2
    DoubleMatrix m1Value = new DoubleMatrix(new double[][] { {1}, {0}});
    DoubleMatrix m2Value = new DoubleMatrix(new double[][] { {0}, {1}});
    DoubleConstantSystem m1System = new DoubleConstantSystem(m1Value);
    DoubleConstantSystem m2System = new DoubleConstantSystem(m2Value);
    this.m1 = new DoubleSystemBuilder(m1System);
    this.m2 = new DoubleSystemBuilder(m2System);
  }
}