MotorContinuousObserver1.java

/*
 * $Id: Motor2ContinuousObserver.java,v 1.4 2008/06/08 04:28:13 koga Exp $
 *
 * Copyright (C) 2004 Koga Laboratory. All rights reserved.
 *
 */
package org.mklab.tool.control.model.matxbook;

import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.tool.control.system.continuous.DoubleBaseContinuousExplicitDynamicSystem;


/**
 * モータと連続時間オブザーバの結合システムを表すクラスです。
 * 
 * @author koga
 * @version $Revision: 1.4 $, 2004/04/23
 */
public class MotorContinuousObserver1 extends DoubleBaseContinuousExplicitDynamicSystem {

  /** モータ */
  Motor motor = new Motor();
  /** 状態フィードバック制御器 */
  StateFeedback stateFeedback = new StateFeedback();
  /** 連続時間オブザーバー */
  ContinuousObserver observer = new ContinuousObserver();

  /**
   * コンストラクター
   */
  public MotorContinuousObserver1() {
    super(2, 0, 3);
  }

  /**
   * {@inheritDoc}
   */
  public DoubleMatrix stateEquation(double t, DoubleMatrix xx, DoubleMatrix uy) {
    DoubleMatrix x = xx.getSubVector(1, 2);
    DoubleMatrix z = xx.getSubVector(3, 3);
    DoubleMatrix u = uy.getSubVector(1, 1);

    DoubleMatrix dx = this.motor.stateEquation(t, x, u);
    DoubleMatrix dz = this.observer.stateEquation(t, z, uy);
    DoubleMatrix dxx = dx.appendDown(dz);
    return dxx;
  }

  /**
   * {@inheritDoc}
   */
  @Override
  public DoubleMatrix inputOutputEquation(double t, DoubleMatrix xx) {
    DoubleMatrix x = xx.getSubVector(1, 2);
    DoubleMatrix z = xx.getSubVector(3, 3);
    DoubleMatrix y = this.motor.outputEquation(t, x);
    DoubleMatrix uh = new DoubleMatrix(1, 1);
    DoubleMatrix uyh = uh.appendDown(y);
    DoubleMatrix xh = this.observer.outputEquation(t, z, uyh);
    DoubleMatrix u = this.stateFeedback.outputEquation(t, xh).unaryMinus();
    DoubleMatrix uy = u.appendDown(y);
    return uy;
  }

  /**
   * {@inheritDoc}
   */
  @Override
  public void setInitialState(DoubleMatrix initialState) {
    DoubleMatrix x0 = initialState.getSubVector(1, 2);
    DoubleMatrix z0 = initialState.getSubVector(3, 3);
    this.motor.setInitialState(x0);
    this.observer.setInitialState(z0);
  }

  /**
   * @see org.mklab.tool.control.system.DynamicSystem#getInitialState()
   */
  @Override
  public DoubleMatrix getInitialState() {
    DoubleMatrix x0 = this.motor.getInitialState();
    DoubleMatrix z0 = this.observer.getInitialState();
    return x0.appendDown(z0);
  }

  /**
   * {@inheritDoc}
   */
  @Override
  public void setState(DoubleMatrix state) {
    DoubleMatrix x = state.getSubVector(1, 2);
    DoubleMatrix z = state.getSubVector(3, 3);
    this.motor.setState(x);
    this.observer.setState(z);
  }

  /**
   * @see org.mklab.tool.control.system.DynamicSystem#getState()
   */
  @Override
  public DoubleMatrix getState() {
    DoubleMatrix x = this.motor.getState();
    DoubleMatrix z = this.observer.getState();
    return x.appendDown(z);
  }

}