!１入力１出力、状態変数がＮ個の１階の連立微分方程式
! 状態方程式 x'(t)=A*x(t)+b*u(t) ※A:N×N、b:N×1
! 出力方程式 y(t)=C*x(t) ※C:1×N
!から
! 伝達関数 G(s)=c*INV(s*I-A)*b=c*adj(s*I-A)*b/det(s*I-A)
!を求める
!行列関連
FUNCTION tr(A(,)) !行列Aのトレース
LET t=0
FOR i=1 TO N
LET t=t+A(i,i)
NEXT i
LET tr=t
END FUNCTION
!多項式関連
SUB mono_disp(ak,k) !単項式を表示する Ak*X^k
IF k<>0 THEN !x^nで
IF ak=1 THEN !係数が１なら
ELSEIF ak=-1 THEN !係数が－１なら
PRINT "-";
ELSE
PRINT STR$(ak);"*";
END IF
END IF
IF k=0 THEN !次数が０なら
PRINT STR$(ak);
ELSEIF k=1 THEN !次数が１なら
PRINT "s";
ELSE
PRINT "s^";STR$(k);
END IF
END SUB
!-------------------- ここまでがサブルーチン
!---------- ↓↓↓↓↓ ----------
LET N=3 !状態変数の数
DATA 0,0,-6 !A
DATA 1,0,-11
DATA 0,1,-6
DATA 1 !b
DATA 0
DATA 0
DATA 0,0,1 !c
!---------- ↑↑↑↑↑ ----------
DIM A(N,N),b(N,1),c(1,N)
MAT READ A
MAT READ b
MAT READ c
!Frame法、Leverrir-Faddeev法
! adj(s*I-A)=s^(n-1)*I+s^(n-2)*β1+ … +s*βn-2+βn-1
! det(s*I-A)=s^n+α1*s^(n-1)+ … + αn-1*s+αn
!のとき
! β0=I、k=1～nについて
! Xk=A*βk-1
! αk=-trace(Xk)/k
! βk=Xk+αk*I
!の逐次計算でαk、βkが求まる。
DIM c1(N) !多項式 c(1)*X^(N-1)+c(2)*X^(N-2)+ … +c(N-1)*X+c(N) の係数
DIM c2(N) !多項式 X^N+c(1)*X^(N-1)+c(2)*X^(N-2)+ … +c(N-1)*X+c(N) の係数
DIM X(N,N),sE(N,N),T1(1,N),T2(1,1) !作業用
MAT X=IDN !adj(s*I-A)
FOR k=1 TO N
!!!MAT PRINT X; !debug
MAT T1=c*X !c*adj(s*I-A)*b ※定数*多項式*定数
MAT T2=T1*b
LET c1(k)=T2(1,1)
MAT X=A*X
LET c2(k)=-tr(X)/k !det(s*I-A)
MAT sE=(c2(k))*IDN
MAT X=X+sE
NEXT k
!!!MAT PRINT c1; !debug
PRINT "分子= ";
FOR i=1 TO N !分子側の多項式を表示する
LET w=c1(i)
IF w>0 THEN PRINT "+";
IF w<>0 THEN CALL mono_disp(w,N-i)
NEXT i
PRINT
!!!MAT PRINT c2; !debug
PRINT "分母= ";
CALL mono_disp(1,N) !分母側の多項式を表示する
FOR i=1 TO N
LET w=c2(i)
IF w>0 THEN PRINT "+";
IF w<>0 THEN CALL mono_disp(w,N-i)
NEXT i
END