mcgrant@elaine2.stanford.edu (Michael Grant) (12/02/90)
The following are a set of programs which accomplish a lot for
me in the way of state-space system manipulation, as well as a
few programs which could be separated from this list to handle
polynomials and the like. The functions that they perform
include:
1) transfer function calculation
2) (which means calculation of det(sI-A) and adj(sI-A))
3) state feedback gain calculations
4) observability matrix and controllability matrix determination
5) manual root finding (in other words, I GUESS, the HP tries it out)
and so forth.
NOTE: AN UNCOMMENTED SET OF THESE PROGRAMS HAS BEEN POSTED IN ANOTHER
POST, SO DON'T WORRY ABOUT EDITING TOO MUCH...
A lot of people wanted the resolvent (sI-A)^-1 program, so I decided
to post the whole darn thing. KEEP IN MIND that these were written
on an HP28s, but I don't think that there are any major compatability
hassles here since we're not dealing with screen manipulation and such.i
Sorry, but while I use these often and I'm almost positive they are correct,
I can't be held responsible for typos. BUT, I'll certainly be glad to
hear from you if you find a problem with any of these programs, because
I want to correct them myself!
Send comments, suggestions, money, fame, etc. to
mcgrant@portia.stanford.edu
Enjoy...(in a manner of speaking)
Michael C. Grant
Information Systems Lab
---
I recommend using the following ORDER command:
{ A B C D INITIALIZE TFNCAL ADJ OBSR CNTR FBGN SNDV POLY
EPOLY POLCH PREM POST tmp idt ns no ni } ORDER
The following four variables are the global variables which
contain the matrices representing your state-space realization
{ A,b,c,d }. They must agree in dimension for the follwing
programs to work properly, and INITIALIZE checks this. In
short, as long as they are meaningful in the vector equation
dx/dt=Ax+Bu, y=Cx+Du, then they are valid.
[[-2 -3 -4]
[ 1 0 0]
[ 0 1 0]]
'A' STO
[1 0 0]
'B' STO
[[3 5 7]]
'C' STO
0
'D' STO
INITIALIZE--confirms the integrity of A,B,C,&D, and sets up the
temporary variables that the rest of the system uses.
INPUTS: none
OUTPUTS: a string, which may contain one or several of the following:
"OK" -- all of the dimensions match and the system is ready.
"AS" -- A is not square.
"AB" -- the rows of A and the rows of B do not match.
"AC" -- the columns of A and the columns of C do not match.
"BD" -- the columns of D and the columns of B do not match.
"CD" -- the rows of C and the rows of D do not match.
COMMENTS: Most of the programs will bomb or give incorrect results if
this program is not run (or does ot return "OK"). I decided
that running this program once is worth the extra nuisance
with respect to the speed savings it generates in the other
programs by eliminating type checking and pre-initializing
the global variables in the system.
<<
<< IF DUP TYPE 1 <=
THEN DROP 1 1
ELSE SIZE LIST-> 1 == 1 IFT
END
>> -> P
<< A P EVAL B P EVAL C P EVAL D P EVAL -> AR AC BR BC CR CC DR DC
<< ""
IF AR AC <>
THEN "AS" +
END
IF BR BC <>
THEN "AB" +
END
IF AC CC <>
THEN "AC" +
END
IF BC DC <>
THEN "BD" +
END
IF CR DR <>
THEN "CD"
END
IF DUP SIZE 0 ==
THEN OK + AR 'ns' STO DC 'ni' STO DR 'no' STO
{ ns ns } 0 CON 'tmp' STO ns IDN 'idt' STO
END
>>
>>
>>
TFNCAL--calculates C[(sI-A)^-1]B
INPUTS: none (assumes INITIALIZE returned "OK")
OUTPUTS: 2: the polynomial list for the numerator, Cadj(sI-A)B
1: the polynomial list for the denominator, det(sI-A)
[ remember, C[(sI-A)^-1]B = Cadj(sI-A)B/det(sI-A) ]
COMMENTS: note that in the multi-input and multi-output cases,
the polynomial in level 2 will have matrix coefficients!
The consequence of this is the POLY will not work for it,
but EPOLY can give you values for individual choices of s.
BUT, in most cases people can use POLCH to choose a
single transfer function polynomial from the matrix.
This program (as well as the ADJ program) uses the method
found in Thomas Kailath, _Linear Systems_, Prentice-Hall,
Englewood Cliffs, N.J, (c) 1980, p. 657, better (or worse)
known as the Levverier-Souriau-Faddeeva-Frame formulas.
<< IF D ABS
THEN D 1
ELSE 0
END ->LIST { 1 } 'tmp' 0 CON 1 -> ai
<< 1 ns
FOR J ai idt * 'tmp' STO+ SWAP C tmp B * *
IF 1 no == 1 ni == AND
THEN 1 GET
END D 'tmp' A STO* 0 1 ns
FOR K 'tmp' { K K } GET +
NEXT J NEG / DUP 'ai' STO * + + SWAP ai +
NEXT
>>
>>
ADJ--calculates (sI-A)^-1
INPUTS: none (assumes INITIALIZE returned "OK")
OUTPUTS: 2: a matrix polynomial list, Adj(sI-A)
1: a scalar polynomial list, det(sI-A)
[ (sI-A)^-1 = adj(sI-A)/det(sI-A) ]
COMMENTS: This is just a stripped-down version of TFNCAL. Because
this was so easy to reduce (and is quite fast), I don't
care that TFNCAL would be equivalent to the following:
<< ADJ SWAP C PREM B POST SWAP >>. I just decided that
modularity can take a backseat so speed; I'm quite
impatient during a test...
Again, POLY does not work with polynomial lists whose
entries (coefficients) are matrices. EPOLY does, however,
because it works for a single value.
<< {} { 1 } 'tmp' 0 CON 1 -> ai
<< 1 ns
FOR J ai idt * 'tmp' STO+ SWAP tmp 'tmp' A STO* 0 1 ns
FOR K 'tmp' { K K } GET +
NEXT J NEG / 'ai' STO + SWAP ai +
NEXT
>>
>>
OBSR--computes the observability matrix of { A,C }
INPUTS: none (assumes INITIALIZE returned "OK")
OUTPUTS: TRN([ C AC A^2C ... (A^(ns-1))C ])
COMMENTS: not much. Even works with multi-ouput systems...
<< C -> X
<< X ARRY-> DROP 2 ns
START X A * DUP 'X' STO ARRY-> DROP
NEXT ns no * ns 2 ->LIST ->ARRY
>>
>>
CNTR--computes the controllability matrix of { A,B }
INPUTS: none (assumes INITIALIZE returned "OK")
OUTPUTS: [ B AB A^2B ... (A^(ns-1))B ]]
COMMENTS: Works for multi-input systems
<< B { ns ni } RDM -> X
<< X TRN ARRY-> DROP 2 ns
START A X * DUP 'X' STO TRN ARRY-> DROP
NEXT ns ni * ns 2 ->LIST ->ARRY TRN
>>
>>
FBGN--calculates the state feedback gains required to
achieve the movement of poles which result in the
given characteristic polynomial. In other words,
this solves for k where det(sI-A+bk)=np.
INPUTS: 1: np - a polynomial list for the new
characteristic polynomial.
OUTPUTS: 1: k - the state-combination vector k.
COMMENTS: Sorry, this only works for a single-input,
single-output system. There are simply too
many degrees of freedom for me to handle in
the multi-input case. In addition, this only
works if the system is controllable. I don't
know what would happen if you specified a
polynomial of degree less than the number of states!
<< -> np
<< { 1 ns } 0 CON ns 1 PUT CNTR INV * np A EPOLY *
>>
>>
SNDV--performs synthetic division on a polynomial list
INPUTS: 2: the polynomial in question, L(x)
1: the root R to try to divide into L(x)
OUTPUTS: 2: the divided polynomial QU(x)
1: the remainder of the division, RM
COMMENTS: you can interpret the results of the division
as L(x)=(x-R)*QU(x)+RM. So, if level 1 contains
a zero, you've just found a root!
<< -> L R
<< L SIZE -> N
<< 0 -> K
<< 1 N
FOR I 'L' I GET R K * + DUP 'K' STO
NEXT
>> -> RM
<< N 1 - ->LIST RM
>>
>>
>>
>>
POLY--returns a polynomal algebraic from a polynomial list
INPUTS: 2: the polynomial list to convert, L(x)
1: the algebraic to substitute into the polynomial, V
OUTPUTS: 1: the polynomial algebraic L(V)
COMMENTS: One would most likely use this to turn a
polynomial list into a SOLVR or PLOT-compatible
algebraic object.
<< -> L V
<< L SIZE -> N
<< 0 1 N
FOR I 'L' I GET V N I - ^ * +
NEXT
>>
>>
>>
EPOLY--evaluate a polynomial list at the given value
INPUTS: 2: the polynomial list in question, L(x)
1: the value to substitute in, M
OUTPUTS: the calculated value, L(M)
COMMENTS: the nice thing about this procedure is that
it will work not only for scalars but for
square matrices.
<< -> L M
<< L SIZE
IF M TYPE DUP 3 == SWAP 4 == OR
THEN M IDM
ELSE 1
END -> N I
<< 0 1 N
FOR J M * 'L' J GET I * +
NEXT
>>
>>
>>
POLCH--get a single value from each matrix in a list
INPUTS: 2: a list of vectors or matrices, L
1: the index list which results in a
valid GET command for each matrix, N
OUTPUTS: { (L(1))(N) (L(2))(N) (L(3))(N) ... (L(n))(N) }
COMMENTS: You may be wondering why this is useful...the
transfer function for a multi-input, multi-output
system has a transfer function MATRIX: the numerator
contains polynomials of s whose coefficients are
matrices! Using POLCH will allow you to extract
and individual transfer function from a single input
to a single output. For example, to find the transfer
function to output 3 from input 2, use { 3 2 } as your
parameter N. In general, use { output# input# } as
your input.
<< -> L N
<< L 1 L SIZE
FOR J 'L' J GET N GET J SWAP PUT
NEXT
>>
>>
PREM--Pre-multiply a list of matrices by a value
INPUTS: 2: The list of scalars or matrices in question, L
1: the scalar or matrix to multiply by, M
OUTPUTS: { M*L(1) M*L(2) ... M*L(n) }
COMMENTS: the ADJ function returns a list of matrices,
so you may wish to use this function to pre-
multiply by your row-matrices.
<< -> L M
<< L 1 L SIZE
FOR J M 'L' J GET *
IFERR DUP SIZE
THEN
ELSE
IF DUP { 1 } == SWAP { 1 1 } == OR
THEN 1 GET
END
END J SWAP PUT
NEXT
>>
>>
POSTM--post-multiply a list of scalars or matrices
INPUTS: 2: the list of scalars or matrices L
1: the scalar or matrix to post-multiply, M
OUTPUTS: { L(1)*M L(2)*M L(3)*M ... L(n)*M }
COMMENTS: See PREM, ADJ, and TFNCAL.
<< -> L M
<< L 1 L SIZE
FOR J 'L' J GET M *
IFERR DUP SIZE
THEN
ELSE
IF DUP { 1 } == SWAP { 1 1 } == OR
THEN 1 GET
END
END J SWAP PUT
NEXT
>>
>>
These variables are updated by INITIALIZE, TFNCAL, etc. and are
necessary for their operation. While INITIALIZE will create them
if necessary, it is advisable to create them yourself so that
you can ORDER them to the end of your directory (out of sight).
[[0]]
'tmp' STO
[[1]]
'idt' STO
1
'ns' STO
1
'no' STO
1
'ni' STO
---