MotorContinuousObserver2.java

/*
 * $Id: Motor2ContinuousObserver2.java,v 1.3 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;


/**
 * モータと連続時間オブザーバーの結合システム(入力、出力、推定状態)を表すクラスです。
 * 
 * @author koga
 * @version $Revision: 1.3 $, 2004/04/23
 */
public class MotorContinuousObserver2 extends MotorContinuousObserver1 {
  /**
   * {@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 io = u.appendDown(y).appendDown(xh);
    return io;
  }
}