DoubleBlockContinuousExplicitDynamicSystem.java

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

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

import java.util.List;

import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.matrix.DoubleMatrixAdapter;
import org.mklab.nfc.matrix.misc.DiagonalMatrix;
import org.mklab.nfc.ode.EquationSolver;
import org.mklab.nfc.ode.SolverStopException;
import org.mklab.tool.control.system.DoubleSystemOperator;


/**
 * 陽的微分方程式で表現されるブロック連続時間動的システムを表わすクラスです。
 * 
 * @author koga
 * @version $Revision$
 */
public class DoubleBlockContinuousExplicitDynamicSystem extends DoubleBlockContinuousDynamicSystem implements DoubleContinuousExplicitDynamicSystem {

  /** M行列 */
  private DoubleMatrix matrixM;
  
  /** 微分代数方程式で表せるシステムならばtrue */
  private boolean isDifferentialAlgebraicSystem = false;
  
  /** ヤコビ行列を持っていればtrue */
  private boolean hasJacobianMatrix = false;

  /** 状態と状態の微分の初期値が整合していればtrue */
  private boolean hasConsistentInitialValue = false;

  /**
   * 新しく生成された<code>BlockContinuousDynamicSystem</code>オブジェクトを初期化します。
   * 
   * @param elements 隣接行列
   * @param inputNodes 入力ノードの番号のリスト(番号は1から始まります)
   * @param outputNodes 出力ノードの番号のリスト(番号は1から始まります)
   */
  public DoubleBlockContinuousExplicitDynamicSystem(final DoubleSystemOperator[][] elements, final List<Integer> inputNodes, final List<Integer> outputNodes) {
    super(elements, inputNodes, outputNodes);
    
    for (final DoubleContinuousDynamicSystem system : this.continuousDynamicSystems) {
      if (((DoubleContinuousExplicitDynamicSystem)system).isDifferentialAlgebraicSystem()) {
        setDifferentialAlgebraicSystem(true);
        break;
      }
    }
    
    final DoubleMatrix[] matricesM = new DoubleMatrix[this.continuousDynamicSystems.size()];
    int count = 0;
    for (final DoubleContinuousDynamicSystem system : this.continuousDynamicSystems) {
      matricesM[count++] = ((DoubleContinuousExplicitDynamicSystem)system).getMatrixM();
    }
    this.matrixM = DiagonalMatrix.create(matricesM);
    
    setHasJacobianMatrix(true);

    setHasConsistentInitialValue(true);
    for (final DoubleContinuousDynamicSystem system : this.continuousDynamicSystems) {
      if (((DoubleContinuousExplicitDynamicSystem)system).hasConsistentInitialValue() == false) {
        setHasConsistentInitialValue(false);
        break;
      }
    }
  }

  /**
   * {@inheritDoc}
   */
  public DoubleMatrix stateEquation(final double t, final DoubleMatrix x,  final DoubleMatrix u) throws SolverStopException {
    setState(x);

    DoubleMatrix dx = new DoubleMatrix(0, 1);
    for (final DoubleContinuousDynamicSystem system : this.continuousDynamicSystems) {
      dx = dx.appendDown(((DoubleContinuousExplicitDynamicSystem)system).stateEquation(t, system.getState(), getInputNodeValueOf((DoubleSystemOperator)system)));
    }

    return dx;
  }
  
  /**
   * {@inheritDoc}
   */
  public DoubleMatrix matrixM() throws SolverStopException {
    final int size = this.continuousDynamicSystems.size();
    final DoubleMatrix[] matrices = new DoubleMatrix[size];
    int count = 0;
    for (final DoubleContinuousDynamicSystem system : this.continuousDynamicSystems) {
      matrices[count++] = ((DoubleContinuousExplicitDynamicSystem)system).matrixM();
    }

    return DiagonalMatrix.create(matrices);
  }

  /**
   * {@inheritDoc}
   */
  public DoubleMatrix differentialEquation(final double t, final DoubleMatrix x, final DoubleMatrix inputOutput) throws SolverStopException {
    final DoubleMatrix u = inputOutput.getSubVector(1, getInputSize());
    return stateEquation(t, x, u);
  }
  
  /**
   * {@inheritDoc}
   */
  public DoubleMatrix getMatrixM() {
    return this.matrixM;
  }
  
  /**
   * {@inheritDoc}
   */
  
  public DoubleMatrix getJacobianMatrix(double t, DoubleMatrix x, DoubleMatrix u) {
    final DoubleMatrix[] jacobians = new DoubleMatrix[this.continuousDynamicSystems.size()];
    int count = 0;
    for (final DoubleContinuousDynamicSystem system : this.continuousDynamicSystems) {
      if (((DoubleContinuousExplicitDynamicSystem)system).hasJacobianMatrix()) {
        jacobians[count++] = ((DoubleContinuousExplicitDynamicSystem)system).getJacobianMatrix(t, system.getState(), getInputNodeValueOf((DoubleSystemOperator)system));
      } else {
        final DoubleMatrix state = system.getState();
        final DoubleMatrix clonedState = system.getState();
        jacobians[count++] = calculateNumericalJacobian((DoubleContinuousExplicitDynamicSystem)system, t, clonedState, getInputNodeValueOf((DoubleSystemOperator)system));
        system.setState(state);
      }
    }

    final DoubleMatrix jacobianMatrix = DiagonalMatrix.create(jacobians);
    return jacobianMatrix;
  }
  
  /**
   * 数値的ヤコビ行列を返します。
   * @param system 対象となるシステム
   * @param t 時刻
   * @param x 状態
   * @param u 入力
   * @return 数値的ヤコビ行列
   */
  private DoubleMatrix calculateNumericalJacobian(DoubleContinuousExplicitDynamicSystem system, double t, DoubleMatrix x, DoubleMatrix u) {
    final int n = system.getState().length();
    
    final double[] F0 = calculateF0(system, t, x, u);
    final double[] FJ0 = new double[n]; // calculateEstimatedFJ0(system, t, x, u);

    final double uround = 1.0e-16;
    final double[][] J0 = new double[n][n];    
    final double[][] x0 = DoubleMatrixAdapter.getElements(x);
    
    for (int i = 1; i <= n; i++) {
      final double ysafe = x0[i - 1][0];
      final double delt = Math.sqrt(uround * Math.max(1e-5, Math.abs(ysafe)));
      x0[i - 1][0] = ysafe + delt;
      
      try {
        EquationSolver.setTrial(true);
        final DoubleMatrix dx = system.differentialEquation(t, x, u);
        EquationSolver.setTrial(false);

        for (int j = 0; j < n; j++) {
          FJ0[j] = dx.getDoubleElement(j + 1, 1);
        }
      } catch (SolverStopException e) {
        throw new RuntimeException(e);
      }
      
      for (int j = 0; j < n; j++) {
        J0[j][i - 1] = (FJ0[j] - F0[j]) / delt;
      }

      x0[i - 1][0] = ysafe;
    }
    
    final DoubleMatrix jacobian = new DoubleMatrix(J0);
    return jacobian;
  }
  
  /**
   * 時刻tにおけるfを返します。
   * @param system 対象となるシステム
   * @param t 時刻
   * @param x 状態
   * @param u 入力
   * @return 時刻tにおけるf
   */
  private double[] calculateF0(DoubleContinuousExplicitDynamicSystem system, double t, DoubleMatrix x, DoubleMatrix u) {
    final int n = system.getState().length();
    final double[] f0 = new double[n]; 
    
    try {
      EquationSolver.setTrial(true);
      final DoubleMatrix dx = system.differentialEquation(t, x, u);
      EquationSolver.setTrial(false);

      for (int i = 0; i < n; i++) {
        f0[i] = dx.getDoubleElement(i + 1, 1);
      }

    } catch (SolverStopException e) {
      throw new RuntimeException(e);
    }
    
    return f0;
  }

  /**
   * {@inheritDoc}
   */
  public boolean isDifferentialAlgebraicSystem() {
    return this.isDifferentialAlgebraicSystem;
  }
  
  /**
   * {@inheritDoc}
   */
  public boolean hasJacobianMatrix() {
    return this.hasJacobianMatrix;
  }
  
  /**
   * {@inheritDoc}
   */
  public boolean hasConsistentInitialValue() {
    return this.hasConsistentInitialValue;
  }
  
  /**
   * 微分代数方程式で表されるシステムであるか設定します。
   * @param isDifferentialAlgebraicSystem 微分代数方程式で表されるシステムならばtrue
   */
  protected void setDifferentialAlgebraicSystem(boolean isDifferentialAlgebraicSystem) {
    this.isDifferentialAlgebraicSystem = isDifferentialAlgebraicSystem;
  }
  
  /**
   * ヤコビ行列を持っているか設定します。
   * @param hasJacobianMatrix ヤコビ行列をもっていればtrue
   */
  protected void setHasJacobianMatrix(boolean hasJacobianMatrix) {
    this.hasJacobianMatrix = hasJacobianMatrix;
  }
  
  /**
   * 状態と状態の微分の初期値が整合しているか設定します。
   * @param hasConsistentInitialValue 状態と状態の微分の初期値が整合していればtrue
   */
  protected void setHasConsistentInitialValue(boolean hasConsistentInitialValue) {
    this.hasConsistentInitialValue = hasConsistentInitialValue;
  }
}