This is chap7.m in view mode; [Download] [Up]
(*
**********************************************************
COSY_PAK package chap7.m
**********************************************************
*)
If [ TrueQ[ $VersionNumber >= 2.0 ],
Off[ General::spell ];
Off[ General::spell1 ] ]
(* B E G I N P A C K A G E *)
BeginPackage["COSYPAK`chap7`",
"LinearAlgebra`MatrixManipulation`",
"SignalProcessing`Analog`LaPlace`",
"SignalProcessing`Analog`InvLaPlace`"]
matrixpower::usage =
"matrixpower[A,n] returns the result of A^n."
(* Mathematica's multiplication of A^2 != A.A *)
(* made function to fix this *)
Controllable::usage =
"Controllable[A, B] returns a logic value
representing the complete state
controllability of system A, B."
PolePlaceGain::usage =
"PolePlaceGain[A, B, poles] returns gain matirix K
to place poles of system A - BK at
locations specified by poles."
ObsPolePlace::usage =
"ObsPolePlace[A, C, newpoles] determines
state observer gain matrix using
Ackermann's formula."
rank::usage =
"rank[A] returns rank of matrix A."
Observable::usage =
"Observable[A, C] returns gain matrix to
place observer poles."
sspace::usage =
"sspace[a, b] returns the state-space form of
the ordinary differential equation such as
y''' + a2 y'' + a3 y' + a4 y = b1 u' + b2 u
with the coefficients of the y variables
given by the list a and the coefficients
of the u variables given by the list b.
The list a must start with a 1 and the list
b should be padded with leading zeroes
if it is not the same length as a. The A,
B, C, and D matrices of the equations
x'= Ax + Bu
y = Cy + Du
as the global values AOUT, BOUT, COUT,
and DOUT."
ExpAt::usage=
"ExpAt[a] returns exponential matrix of A."
SysResponse::usage=
"SysResponse[A, B, C, x0, input, s, {t, tmin, tmax}, gopts]: \
Graphs output of system y and returns time domain \
solutions for state x and output y. Returns graphics."
OutControllable::usage=
"OutControllable[A, B, C, D] returns a logic value
representing the output
controllability of system A, B, C, D."
tpose::usage=
"tpose[A] returns the transpose of
the matrix A."
Begin["`private`"]
rank[a_]:= Block[{red,size,rnk,nz,i,j},
rnk= 0;
size= Dimensions[a];
red= RowReduce[a];
Do[ nz= False;
Do[ If [red[[i,j]]!= 0, nz= True],
{j,size[ [2] ]} ];
If [ nz, rnk= rnk + 1], {i, size[ [1] ]} ];
rnk
];
matrixpower[matrix_,power_Integer?Positive]:=
Block[ {ii,temp,size},
size= Dimensions[matrix];
temp= IdentityMatrix[ size[ [1] ] ];
Do[ temp= temp . matrix, {ii,power} ];
temp
];
Controllable[a_,b_]:=Block[{i, augm, dimena, rowa, cola,
dimenb, rowb, colb},
(* Get sizes of matrices *)
dimena= Dimensions[a];
dimenb= Dimensions[b];
rowa= dimena[ [1] ];
cola= dimena[ [2] ];
rowb= dimenb[ [1] ];
colb= dimenb[ [2] ];
If[rowa != cola, Print["A must be a square matrix."]];
augm= b;
(* Form augmented matrix *)
Do[augm= AppendRows[augm,matrixpower[a,i].b],
{i,(rowa-1)}];
(* Check if controllability *)
(* matrix is of full rank *)
rank[augm] == rowa
];
PolePlaceGain[a_,b_,newpoles_]:= Block[{m1, s, dimnpoles,
phi, i, tot, temp, j, dimena, rowa, m2},
If[ Controllable[a,b],
dimnpoles= Dimensions[newpoles];
(* Ackermann's formula *)
phi= Expand[ Product[ s - newpoles[ [i] ],
{ i,dimnpoles[ [1] ] } ] ];
tot= Coefficient[phi,s,0] IdentityMatrix
[dimnpoles[ [1] ] ];
Do[tot= tot +
Coefficient[phi,s,i] matrixpower[a,i],
{i,1,dimnpoles[ [1] ]} ];
If[ dimnpoles[ [1] ] == 1, m1= {1}, m1= {0};
Do [AppendTo[m1,0], {i, 1, (dimnpoles[[1]]-2)} ];
AppendTo[ m1,1 ]; ];
m1= {m1};
(* controllability matrix *)
dimena= Dimensions[ a ];
rowa= dimena[ [1] ];
m2= b;
Do[m2= AppendRows[m2, matrixpower[a,i].b],
{i,(rowa-1)} ];
(* find gain matrix K *)
m1 . Inverse[ m2 ] . tot,
(* Not Controllable *)
Print["The system is not controllable."]]
];
ObsPolePlace[a_,c_,newpoles_]:= Block[{m1, s, dimnpoles,
phi, i, tot, temp, j, dimena, rowa, m2},
(* full state observer *)
If[Observable[a,c],
dimnpoles= Dimensions[newpoles];
(* Ackermann's formula *)
phi= Expand[ Product[ s - newpoles[ [i] ],
{ i,dimnpoles[ [1] ] } ] ];
tot=Coefficient[phi,s,0] IdentityMatrix
[dimnpoles[ [1] ] ];
Do[tot= tot +
Coefficient[phi,s,i] matrixpower[a,i],
{i,1,dimnpoles[ [1] ]} ];
If[ dimnpoles[ [1] ] == 1, m1= {{1}}, m1= {{0}};
Do [ m1= AppendColumns[m1, {{0}} ],
{i, 1, (dimnpoles[[1]]-2)} ];
m1= AppendColumns[ m1, {{1}} ]; ];
(* observability matrix *)
dimena= Dimensions[ a ];
rowa= dimena[ [1] ];
m2= c;
Do[m2= AppendColumns[m2, c . matrixpower[a,i] ],
{i,(rowa-1)} ];
(* find gain matrix K *)
tot . Inverse[ m2 ] . m1,
(* Not Observable *)
Print["The system is not observable."]]
];
tpose[a_]:= Block[{i,j,size,row,col,temp},
(* get # of rows and cols of a *)
size= Dimensions[a];
row= size[[1]];
col= size[[2]];
(* initialize temp matrix *)
temp= Array[1,{col,row}];
(* transpose elements *)
Do[
Do[temp[[j,i]]= a[[i,j]], { j, size[[2]] }],
{ i, size[[1]] }];
temp ];
Observable[a_,c_]:=Block[{i,augm,dimena,rowa,cola,dimenc
,rowc,colc},
(* Get sizes of matrices *)
dimena=Dimensions[a];
dimenc=Dimensions[c];
rowa=dimena[[1]];
cola=dimena[[2]];
rowc=dimenc[[1]];
colc=dimenc[[2]];
If[rowa != cola, Print["A must be a square matrix."]];
(* Begin augmented matrix *)
augm= c;
(* Form augmented matrix *)
Do[augm= AppendColumns[augm,
c . matrixpower[a,i] ],
{i,(rowa-1)} ];
(* Check if observability *)
(* matrix is of full rank *)
rank[augm] == rowa
];
sspace[a_,b_]:= Block[{na,nb,beta,temp},
(* Convert ordinary differential *)
(* equation to state space *)
(* find size of input lists *)
na= Dimensions[a];
na= na[[1]];
nb= Dimensions[b];
nb= nb[[1]];
(* lists must be same size *)
If[nb != na,Print["Coefficient lists a and b",
" must be the same size. Remember to pad",
" the lists with leading zeros, if needed."] ];
beta= {b[[1]]};
Do[ temp= b[[i]];
Do[ temp= temp - a[[j]] beta[[i-j+1]], {j,2,i}];
AppendTo[ beta, temp], {i,2,nb}];
AOUT= IdentityMatrix[na-1];
Do[ Do[ If[ (j-1) == i, AOUT[[i,j]]= 1, AOUT[[i,j]]= 0],
{i, na-2}];
AOUT[[na-1,j]]= -a[[na+1-j]], {j,na-1} ];
BOUT= {{beta[[2]]}};
Do[ BOUT= AppendColumns[ BOUT, {{beta[[i]]}} ],
{i, 3, na}];
COUT= {{1}};
Do[ COUT=AppendRows[COUT,{{0}}], {na-2}];
DOUT= beta[[1]];
Print["The A Matrix is:"];
Print[" "]; Print[" "];
Print[" ",TableForm[AOUT]];
Print[" "]; Print[" "];
Print["The B Matrix is:"];
Print[" "]; Print[" "];
Print[" ",TableForm[BOUT]];
Print["The C Matrix is:"];
Print[" "]; Print[" "];
Print[" ",TableForm[COUT]];
Print[" "]; Print[" "];
Print["The D Matrix is:"];
Print[" "]; Print[" "];
Print[" ",TableForm[DOUT]];
];
OutControllable[a_,b_,c_,d_]:=Block[{i, augm, dimena, dimenc,
rowa, cola, dimenb, rowb, colb, rowc},
(* Get sizes of matrices *)
dimena= Dimensions[a];
dimenb= Dimensions[b];
dimenc= Dimensions[c];
rowa= dimena[ [1] ];
cola= dimena[ [2] ];
rowb= dimenb[ [1] ];
colb= dimenb[ [2] ];
rowc= dimenc[ [1] ];
If[rowa != cola, Print["A must be a square matrix."]];
augm= c.b;
(* Form augmented matrix *)
Do[augm= AppendRows[augm,c.matrixpower[a,i].b],
{i,(rowa-1)}];
AppendRows[augm, d];
(* Check if controllability *)
(* matrix is of full rank *)
rank[augm] == rowc
];
SysResponse[a_, b_, c_, x0_, input_,s_,{t_,tmin_,tmax_}, opts___]:=
Block[{tau, intexp, x, y, dimena,dimenb,rowa,cola,rowb,colb,
dimenc,colc,intu,II,exps},
(* Get sizes of matrices *)
dimena= Dimensions[a];
dimenb= Dimensions[b];
dimenc= Dimensions[c];
rowa= dimena[[1]];
cola= dimena[[2]];
colb= dimenb[[1]];
colc= dimenc[[1]];
If[rowa != cola, Print["expAt must be a square matrix."];
Return[{}]];
If[rowa != colb, Print["expAt and B must have an equal \
number of columns"];Return[{}]];
If[cola != colc, Print["expAt and C must have an equal \
number of rows"];Return[{}]];
II=IdentityMatrix[rowa];
exps =Together[Inverse[s II -a]];
inputs = LaPlace[input,t,s][[1]];
initpart = exps.x0;
Do[initpart[[i]]= Simplify[ComplexExpand[
InvLaPlace[initpart[[i]],s,t, Apart->All]]],{ i, cola }];
forcepart = exps.b inputs;
Do[forcepart[[i]]= Simplify[ComplexExpand[
InvLaPlace[forcepart[[i]],s,t, Apart->All]]],{ i, cola }];
(* integrate and add initial *)
(* conditions to find x and y *)
x= initpart + forcepart;
y= c.x ;
(* output graph and solutions *)
Plot[y,{t,tmin,tmax},
FrameLabel->{"Time","Y(t)","Time Response of System "," "},
Frame->True,PlotRange -> All,GridLines->Automatic,
FrameTicks -> {Automatic, Automatic,Automatic, Automatic},
opts];
Return[{x,y}]
];
ExpAt[a_]:=
Block[{i,j,out,xform, II, nn},
nn = Dimensions[a][[1]];
II= IdentityMatrix[nn];
xform= Inverse[s II - a];
Do[
Do[
out[[i,j]]= Simplify[ComplexExpand[InvLaPlace[ xform[[i,j]],s]]],
{ j, nn}],
{ i, nn}];
Return[out]
];
End[]
EndPackage[]
(* E N D P A C K A G E *)
If [ TrueQ[ $VersionNumber >= 2.0 ],
On[ General::spell ];
On[ General::spell1 ] ]
These are the contents of the former NiCE NeXT User Group NeXTSTEP/OpenStep software archive, currently hosted by Netfuture.ch.