01 /*
02 * $Id: MotorStateFeedbackRK4StepLoop.java,v 1.5 2008/02/02 03:06:25 koga Exp $
03 *
04 * Copyright (C) 2004 Koga Laboratory. All rights reserved.
05 *
06 */
07 package matxbook.chap21;
08
09 import org.mklab.nfc.matrix.DoubleMatrix;
10 import org.mklab.nfc.matrix.Matrix;
11 import org.mklab.nfc.ode.DifferentialEquationSolver;
12 import org.mklab.nfc.ode.SolverStopException;
13 import org.mklab.nfc.ode.RungeKutta4;
14
15
16 /**
17 * モータの状態フィードバック制御のシミュレーションを4次のルンゲ・クッタ法(ステップの繰り返し) で実行するクラスです。
18 *
19 * @author koga
20 * @version $Revision: 1.5 $, 2004/04/23
21 */
22 public class MotorStateFeedbackRK4StepLoop {
23
24 /**
25 * メインメソッド
26 *
27 * @param args コマンドライン引数
28 * @throws SolverStopException ソルバーが停止された場合
29 */
30 public static void main(String[] args) throws SolverStopException {
31 MotorStateFeedback system = new MotorStateFeedback();
32 Matrix x0 = new DoubleMatrix(new double[] {0, 0}).transpose();
33 system.setInitialState(x0);
34
35 DifferentialEquationSolver solver = new RungeKutta4();
36
37 double t0 = 0;
38 double t1 = 10;
39 double h = 0.1;
40 int n = (int)((t1 - t0) / h);
41
42 DoubleMatrix ttData = new DoubleMatrix(1, n + 1);
43 DoubleMatrix xxData = new DoubleMatrix(2, n + 1);
44 DoubleMatrix ioData = new DoubleMatrix(3, n + 1);
45
46 Matrix x = x0;
47 double t = t0;
48 Matrix io = system.inputOutputEquation(t, x);
49 xxData.setColumnVector(1, x);
50 ttData.setElement(1, t);
51 ioData.setColumnVector(1, io);
52
53 for (int i = 1; i <= n; i++) {
54 x = solver.step(system, t, x, h);
55 t = t + h;
56 io = system.inputOutputEquation(t, x);
57 xxData.setColumnVector(i + 1, x);
58 ttData.setElement(i + 1, t);
59 ioData.setColumnVector(i + 1, io);
60 }
61 }
62 }
|