DoubleFirstOrderHoldSystem.java

/*
 * $Id$
 *
 * Copyright (C) 2004-2005 Koga Laboratory. All rights reserved.
 */

package org.mklab.tool.control.system.discrete;

import org.mklab.nfc.matrix.DoubleMatrix;


/**
 * １次ホールドシステムを表すクラスです。
 * 
 * @author koga
 * @version $Revision$
 */
public class DoubleFirstOrderHoldSystem extends DoubleHoldSystem {

  /**
   * {@inheritDoc}
   */
  @Override
  public DoubleMatrix outputEquation(final double t, final DoubleMatrix u) {
    final int size = getInputSize();

    if (isAtSamplingPoint()) {
      DoubleMatrix state = getState();
      setState(u.appendDown(state.getSubVector(1, size)));
    }

    final double interval = getSamplingInterval();
    final DoubleMatrix state = getState();
    final DoubleMatrix u0 = state.getSubVector(1, size);
    final DoubleMatrix u1 = state.getSubVector(size + 1, 2 * size);
    final DoubleMatrix ratio = u0.subtract(u1).divide(interval);
    int k = (int)Math.floor(t / interval);

    if (!isAtSamplingPoint() && (t - k * interval <= 0)) {
      k = k - 1;
    }

    return u0.add(ratio.multiply(t - k * interval));
  }

  /**
   * @see org.mklab.tool.control.system.SystemOperator#initialize()
   */
  @Override
  public void initialize() {
    final int size = getInputSize();

    if (size < 0) {
      return;
    }

    setState(new DoubleMatrix(2 * size, 1));
  }
}