BlockContinuousExplicitDynamicSystem.java
/**
* $Id$
*
* Copyright (C) 2004-2005 Koga Laboratory. All rights reserved.
*/
package org.mklab.tool.control.system.continuous;
import java.util.ArrayList;
import java.util.List;
import org.mklab.nfc.matrix.ComplexNumericalMatrix;
import org.mklab.nfc.matrix.RealNumericalMatrix;
import org.mklab.nfc.matrix.misc.DiagonalMatrix;
import org.mklab.nfc.ode.EquationSolver;
import org.mklab.nfc.ode.SolverStopException;
import org.mklab.nfc.scalar.ComplexNumericalScalar;
import org.mklab.nfc.scalar.RealNumericalScalar;
import org.mklab.tool.control.system.SystemOperator;
/**
* 陽的微分方程式で表現されるブロック連続時間動的システムを表わすクラスです。
*
* @author koga
* @version $Revision$
* @param <RS> type of real scalar
* @param <RM> type of real matrix
* @param <CS> type of complex scalar
* @param <CM> type of complex matrix
*/
public class BlockContinuousExplicitDynamicSystem<RS extends RealNumericalScalar<RS,RM,CS,CM>, RM extends RealNumericalMatrix<RS,RM,CS,CM>, CS extends ComplexNumericalScalar<RS,RM,CS,CM>, CM extends ComplexNumericalMatrix<RS,RM,CS,CM>> extends BlockContinuousDynamicSystem<RS,RM,CS,CM> implements ContinuousExplicitDynamicSystem<RS,RM,CS,CM> {
/** M行列 */
private RM 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から始まります)
* @param sunit unit of scalar
*/
public BlockContinuousExplicitDynamicSystem(final SystemOperator<RS,RM,CS,CM>[][] elements, final List<Integer> inputNodes, final List<Integer> outputNodes, RS sunit) {
super(elements, inputNodes, outputNodes, sunit);
for (final ContinuousDynamicSystem<RS,RM,CS,CM> system : this.continuousDynamicSystems) {
if (((ContinuousExplicitDynamicSystem<RS,RM,CS,CM>)system).isDifferentialAlgebraicSystem()) {
setDifferentialAlgebraicSystem(true);
break;
}
}
final List<RM> matricesM = new ArrayList<>(this.continuousDynamicSystems.size());
//int count = 0;
for (final ContinuousDynamicSystem<RS,RM,CS,CM> system : this.continuousDynamicSystems) {
matricesM.add(((ContinuousExplicitDynamicSystem<RS,RM,CS,CM>)system).getMatrixM());
}
this.matrixM = DiagonalMatrix.create(matricesM);
setHasJacobianMatrix(true);
setHasConsistentInitialValue(true);
for (final ContinuousDynamicSystem<RS,RM,CS,CM> system : this.continuousDynamicSystems) {
if (((ContinuousExplicitDynamicSystem<RS,RM,CS,CM>)system).hasConsistentInitialValue() == false) {
setHasConsistentInitialValue(false);
break;
}
}
}
/**
* {@inheritDoc}
*/
public RM stateEquation(final RS t, final RM x, final RM u) throws SolverStopException {
setState(x);
RM dx = this.sunit.createZeroGrid(0, 1);
for (final ContinuousDynamicSystem<RS,RM,CS,CM> system : this.continuousDynamicSystems) {
dx = dx.appendDown(((ContinuousExplicitDynamicSystem<RS,RM,CS,CM>)system).stateEquation(t, system.getState(), getInputNodeValueOf((SystemOperator<RS,RM,CS,CM>)system)));
}
return dx;
}
/**
* {@inheritDoc}
*/
public RM matrixM() throws SolverStopException {
final int size = this.continuousDynamicSystems.size();
final List<RM> matrices = new ArrayList<>(size);
//int count = 0;
for (final ContinuousDynamicSystem<RS,RM,CS,CM> system : this.continuousDynamicSystems) {
matrices.add(((ContinuousExplicitDynamicSystem<RS,RM,CS,CM>)system).matrixM());
}
return DiagonalMatrix.create(matrices);
}
/**
* {@inheritDoc}
*/
public RM differentialEquation(final RS t, final RM x, final RM inputOutput) throws SolverStopException {
final RM u = inputOutput.getSubVector(1, getInputSize());
return stateEquation(t, x, u);
}
/**
* {@inheritDoc}
*/
public RM getMatrixM() {
return this.matrixM;
}
/**
* {@inheritDoc}
*/
public RM getJacobianMatrix(RS t, RM x, RM u) {
final List<RM> jacobians = new ArrayList<>(this.continuousDynamicSystems.size());
//int count = 0;
for (final ContinuousDynamicSystem<RS,RM,CS,CM> system : this.continuousDynamicSystems) {
if (((ContinuousExplicitDynamicSystem<RS,RM,CS,CM>)system).hasJacobianMatrix()) {
jacobians.add(((ContinuousExplicitDynamicSystem<RS,RM,CS,CM>)system).getJacobianMatrix(t, system.getState(), getInputNodeValueOf((SystemOperator<RS,RM,CS,CM>)system)));
} else {
final RM state = system.getState();
final RM clonedState = system.getState();
jacobians.add(calculateNumericalJacobian((ContinuousExplicitDynamicSystem<RS,RM,CS,CM>)system, t, clonedState, getInputNodeValueOf((SystemOperator<RS,RM,CS,CM>)system)));
system.setState(state);
}
}
final RM jacobianMatrix = DiagonalMatrix.create(jacobians);
return jacobianMatrix;
}
/**
* 数値的ヤコビ行列を返します。
* @param system 対象となるシステム
* @param t 時刻
* @param x 状態
* @param u 入力
* @return 数値的ヤコビ行列
*/
private RM calculateNumericalJacobian(ContinuousExplicitDynamicSystem<RS,RM,CS,CM> system, RS t, RM x, RM u) {
final int n = system.getState().length();
final RS[] F0 = calculateF0(system, t, x, u);
final RS[] FJ0 = this.sunit.createArray(n); // calculateEstimatedFJ0(system, t, x, u);
final RS uround = this.sunit.getMachineEpsilon(); // 1.0e-16;
final RS[][] J0 = this.sunit.createArray(n, n);
final RS[][] x0 = x.getElements();
for (int i = 1; i <= n; i++) {
final RS ysafe = x0[i - 1][0];
final RS delt = uround.multiply(ysafe.abs().max(1e-5)).sqrt();
x0[i - 1][0] = ysafe.add(delt);
try {
EquationSolver.setTrial(true);
final RM dx = system.differentialEquation(t, x, u);
EquationSolver.setTrial(false);
for (int j = 0; j < n; j++) {
FJ0[j] = dx.getElement(j + 1, 1);
}
} catch (SolverStopException e) {
throw new RuntimeException(e);
}
for (int j = 0; j < n; j++) {
J0[j][i - 1] = FJ0[j].subtract(F0[j]).divide(delt);
}
x0[i - 1][0] = ysafe;
}
final RM jacobian = this.sunit.createGrid(J0);
return jacobian;
}
/**
* 時刻tにおけるfを返します。
* @param system 対象となるシステム
* @param t 時刻
* @param x 状態
* @param u 入力
* @return 時刻tにおけるf
*/
private RS[] calculateF0(ContinuousExplicitDynamicSystem<RS,RM,CS,CM> system, RS t, RM x, RM u) {
final int n = system.getState().length();
final RS[] f0 = this.sunit.createArray(n);
try {
EquationSolver.setTrial(true);
final RM dx = system.differentialEquation(t, x, u);
EquationSolver.setTrial(false);
for (int i = 0; i < n; i++) {
f0[i] = dx.getElement(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;
}
}