001 /*
002 * $Id: Motor2ContinuousObserver.java,v 1.4 2008/06/08 04:28:13 koga Exp $
003 *
004 * Copyright (C) 2004 Koga Laboratory. All rights reserved.
005 *
006 */
007 package matxbook.chap21;
008
009 import org.mklab.nfc.matrix.Matrix;
010 import org.mklab.tool.control.system.continuous.BaseContinuousDynamicSystem;
011
012
013 /**
014 * モータと連続時間オブザーバの結合システムを表すクラスです。
015 *
016 * @author koga
017 * @version $Revision: 1.4 $, 2004/04/23
018 */
019 public class Motor2ContinuousObserver extends BaseContinuousDynamicSystem {
020 /** モータ */
021 Motor2 motor = new Motor2();
022 /** 状態フィードバック制御器 */
023 StateFeedback stateFeedback = new StateFeedback();
024 /** 連続時間オブザーバー */
025 ContinuousObserver observer = new ContinuousObserver();
026
027 /**
028 * コンストラクター
029 */
030 public Motor2ContinuousObserver() {
031 super(2, 0, 3);
032 }
033
034 /**
035 * @see org.mklab.tool.control.system.continuous.ContinuousDynamicSystem#stateEquation(double, org.mklab.nfc.matrix.Matrix, org.mklab.nfc.matrix.Matrix)
036 */
037 public Matrix stateEquation(double t, Matrix xx, Matrix uy) {
038 Matrix x = xx.getSubVector(1, 2);
039 Matrix z = xx.getSubVector(3, 3);
040 Matrix u = uy.getSubVector(1, 1);
041
042 Matrix dx = this.motor.stateEquation(t, x, u);
043 Matrix dz = this.observer.stateEquation(t, z, uy);
044 Matrix dxx = dx.appendDown(dz);
045 return dxx;
046 }
047
048 /**
049 * @see org.mklab.nfc.ode.DifferentialSystem#inputOutputEquation(double, org.mklab.nfc.matrix.Matrix)
050 */
051 @Override
052 public Matrix inputOutputEquation(double t, Matrix xx) {
053 Matrix x = xx.getSubVector(1, 2);
054 Matrix y = this.motor.outputEquation(t, x);
055 Matrix z = xx.getSubVector(3, 3);
056 Matrix xh = this.observer.outputEquation(t, z, y);
057 Matrix u = this.stateFeedback.outputEquation(t, xh);
058 Matrix uy = u.appendDown(y);
059 return uy;
060 }
061
062 /**
063 * @see org.mklab.tool.control.system.DynamicSystem#setInitialState(org.mklab.nfc.matrix.Matrix)
064 */
065 @Override
066 public void setInitialState(Matrix initialState) {
067 Matrix x0 = initialState.getSubVector(1, 2);
068 Matrix z0 = initialState.getSubVector(3, 3);
069 this.motor.setInitialState(x0);
070 this.observer.setInitialState(z0);
071 }
072
073 /**
074 * @see org.mklab.tool.control.system.DynamicSystem#getInitialState()
075 */
076 @Override
077 public Matrix getInitialState() {
078 Matrix x0 = this.motor.getInitialState();
079 Matrix z0 = this.observer.getInitialState();
080 return x0.appendDown(z0);
081 }
082
083 /**
084 * @see org.mklab.tool.control.system.DynamicSystem#setState(org.mklab.nfc.matrix.Matrix)
085 */
086 @Override
087 public void setState(Matrix state) {
088 Matrix x = state.getSubVector(1, 2);
089 Matrix z = state.getSubVector(3, 3);
090 this.motor.setState(x);
091 this.observer.setState(z);
092 }
093
094 /**
095 * @see org.mklab.tool.control.system.DynamicSystem#getState()
096 */
097 @Override
098 public Matrix getState() {
099 Matrix x = this.motor.getState();
100 Matrix z = this.observer.getState();
101 return x.appendDown(z);
102 }
103
104 }
|