%%HP: T(3)A(D)F(.);
@ Optimal root placement for Linear-Quadratic Optimum Controll
@ LQPOLES
@ by James Walters
DIR
PG4
DIR
MAIN
DIR
EE
DIR
CNTRL
DIR
STVR
DIR
@ For the optimal pole assignment program, begin with a system
@ dX/dt = A*X+B*u, y = C*X+D*u
@ and a cost function J = integral( TRN(X)*Q*X + R*u^2)
@ to be minimized.
@ Store A, B, C, D, Q, and R matrices as variables.
@
@ run OPRTS.
@ On the stack will be the optimal feedback gain matrix g.
@ The optimal closed loop system is of the form
@ dX/dt = A*X + B*(-g*X)
@ New variables Qo, Qc, H, W, a, ahat, n are also formed by this program
@
@ Included below is both the main program, and a sample system
@
@ The optimal roots should be -35, -91.6, -12.57
@
OPRTS @ OPTIMAL ROOTS PLACEMENT
\<< HAMLTN @ form hamiltonian matrix
CHEQ ROOTS 2 n * @ find roots of hamiltonian
\->LIST \-> HROOTS
\<< 1 n 2 *
FOR I @ keep roots with negative real parts
HROOTS I GET DUP
IF RE
0 >
THEN
DROP
END
NEXT
\>> n \->LIST @ place optimal roots into list
IRT LTOV 'ahat' STO @ transform optimal roots into polynomial vector ahat
POLEPL g @ place roots of system at optimal position, return optimal
\>> @ gain matrix on stack.
POLEPL @ POLE PLACEMENT PROGRAM
\<< A CHEQ @ find characteristic equation of system
DUP SIZE 1 - 'n'
STO DUP LTOV 'a' @ place eq in vector form
STO n IDN 'W' STO 2 @ form wronskian matrix W
n
FOR i DUP
i GET i n
FOR j
DUP 'W(j-i+1,j)'
STO
NEXT
DROP
NEXT DUP
CNTRLBL DROP Qc W * @ do controllability test, generate Qc
INV TRN ahat a - * @ determine gain matrix g to place poles at desired
EVAL TRN 'g' STO @ location. Store gain matrix as g.
\>>
OBSVBL @ OBSERVABILITY MATRIX PROGRAM
\<< n IDN DUP
'Qo' STO 1 n
FOR j DUP
C TRN * 1 n
FOR i
DUP i 1 2 \->LIST GET
Qo SWAP i j 2 \->LIST
SWAP PUT 'Qo' STO
NEXT
DROP A TRN *
NEXT DROP
Qo DET 0 \=/
\>>
CNTRLBL @ CONTROLLABILITY MATRIX PROGRAM
\<< n IDN DUP
'Qc' STO 1 n
FOR j DUP
B * 1 n
FOR i
DUP i 1 2 \->LIST GET
Qc SWAP i j 2 \->LIST
SWAP PUT 'Qc' STO
NEXT
DROP A *
NEXT DROP
Qc DET 0 \=/
\>>
HAMLTN @ HAMILTONIAN MATRIX PROGRAM
\<< B R INV *
B TRN * -1 * \-> tmp
\<< n 2 *
IDN 'H' STO H 1 n
FOR i 1
n
FOR j
A i j 2 \->LIST GET i
j 2 \->LIST SWAP PUT
Q i j 2 \->LIST GET
-1 * i n + j 2
\->LIST SWAP PUT A j
i 2 \->LIST GET -1 *
i n + j n + 2 \->LIST
SWAP PUT tmp i j 2
\->LIST GET i j n + 2
\->LIST SWAP PUT
NEXT
NEXT
'H' STO H
\>>
\>>
LTOV @ LIST TO COLUMN VECTOR PROGRAM
\<< DUP SIZE
1 - DUP 1 2 \->LIST 0
CON \-> n a
\<< 2 n 1 +
FOR i
DUP i GET RE 'a(i-1,1)' STO
NEXT
DROP a
\>>
\>>
H @ EXAMPLE HAMILTONIAN MATRIX
[[ 91 2 3 -1 -2 -3 ]
[ 4 5 65 -2 -4 -6 ]
[ 7 8 19 -3 -6 -9 ]
[ -1 0 0 -91 -4 -7 ]
[ 0 -1 0 -2 -5 -8 ]
[ 0 0 -1 -3 -65 -19 ]]
Q @ EXAMPLE STATE VARIABLE WEIGHTING MATRIX
[[ 1 0 0 ]
[ 0 1 0 ]
[ 0 0 1 ]]
R 1 @ EXAMPLE CONTROL WEIGHT
g @ EXAMPLE STATE VARIBALE FEEDBACK MATRIX
[[ 335.453288908 -2.61764442827 -25.3230917657 ]]
W @ EXAMPLE WRONSKIAN MATRIX
[[ 1 -115 1730 ]
[ 0 1 -115 ]
[ 0 0 1 ]]
a @ EXAMPLE SYSTEM CHARACTERISTIC VECTOR
[[ -115 ]
[ 1730 ]
[ 37926 ]]
ahat @ EXAMPLE OPTIMAL CHARACTERISTIC VECTOR
[[ 139.248724755 ]
[ 4805.60367291 ]
[ 40398.0276333 ]]
Qo @ EXAMPLE OBSERVABILITY MATRIX
[[ 1 8 132 ]
[ 0 10 162 ]
[ 1 12 192 ]]
C @ C MATRIX OF y = C*X+D*u
[[ 1 0 1 ]]
Qc @ EXAMPLE CONTROLLABILITY MATRIX
[[ 1 104 10122 ]
[ 2 209 6661 ]
[ 3 80 3920 ]]
n 3 @ SYSTEM ORDER
B @ dXdt = A*X+B*u
[[ 1 ]
[ 2 ]
[ 3 ]]
A @ dXdt = A*X+B*u
[[ 91 2 3 ]
[ 4 5 65 ]
[ 7 8 19 ]]
END @ END OF STATE VARIABLE DIRECTORY
END @ END OF CNTRL DIRECTORY
END @ END OF EE DIR
END @ END OF MAIN DIR
CHEQ @ FINDS CHARACTERISTIC EQUATION OF A MATRIX
\<< TRCN SYM
\>>
LVCT @
\<< \-> n
\<< n 1
\->LIST 0 CON n 1
FOR i i
1 \->LIST 3 ROLL PUT
-1
STEP
\>>
\>>
GSO
\<< DUP SIZE
LIST\-> DROP DUP DUP
2 + ROLLD \->LIST \-> M
\<< 2 SWAP
FOR n M
n GET 1 n 1 -
FOR i
M i GET DUP DUP2
DOT INV * SWAP 3
PICK DOT * -
NEXT
n M SWAP ROT PUT
'M' STO
NEXT M
LIST\-> DROP
\>>
\>>
TRCN
\<< DUP SIZE
1 GET \-> g n
\<< g 'tmp'
STO { } 1 n
START 0
1 n
FOR i
tmp i DUP 2 \->LIST
GET +
NEXT
1 \->LIST + 'tmp' g
STO*
NEXT
'tmp' PURGE
\>>
\>>
SYM
\<< DUP SIZE
\-> b n
\<< { 1 } 1
n
FOR i \->
s
\<< 0 1
i
FOR j b j GET s i j
- 1 + GET * -
NEXT i / 1 \->LIST s
SWAP +
\>>
NEXT
\>>
\>>
PSERS
\<< \-> X
\<< LIST\-> 0
SWAP 1
FOR n n
1 + ROLL X n 1 - ^
* + -1
STEP
\>>
\>>
CST { UP MAIN
EXCO ROOTS PADD
PMUL PDIV SYM\-> \->SYM
IRT CST PG5 CHEQ
\->VCT }
END @ END OF PG4 DIRECTORY
ROOTS @ Finds roots of any polynomial
\<< TRIM DUP SIZE
\-> n
\<<
IF n 5 >
THEN DUP
BAIRS SWAP OVER
PDIV DROP \-> A B
\<< A ROOTS
B ROOTS
\>>
ELSE PROOT
END
\>>
\>>
EXCO @ Expand completely algebraic
\<<
\<< EXPAN
\>> MULTI
\<< COLCT
\>> MULTI
\>>
PADD @ Adds to polynomial lists
\<< DUP2 SIZE
SWAP SIZE \-> A B nB
nA
\<< A L\178A B L\178A
IF nA nB <
THEN SWAP
END
IF nA nB \=/
THEN 1 nA
nB - ABS
START 0
NEXT
END nA nB -
ABS 1 + ROLL OBJ\-> 1
GET nA nB - ABS +
\->ARRY + L\178A
\>>
\>>
PMUL @ Multiplies polynomial lists
\<< DUP2 SIZE
SWAP SIZE \-> A B nB
nA
\<< { }
IF nB 1 >
THEN 2 nB
START 0 +
NEXT
END DUP 'A'
STO+ 'A' SWAP STO+
A OBJ\-> \->ARRY B OBJ\->
DROP
IF nB 1 >
THEN 2 nB
FOR J J
ROLL
NEXT
END
IF 3 nA nB
+ \<=
THEN 3 nA
nB +
START 0
NEXT
END nA nB 1
- 2 * + \->ARRY 2 nA
nB +
START DUP2
DOT 3 ROLLD OBJ\->
SWAP nA nB 1 - 2 *
+ 1 + ROLLD \->ARRY
NEXT DROP2
nA nB + 1 - \->LIST
\>>
\>>
PDIV
\<< DUP SIZE 3
ROLLD OBJ\-> \->ARRY
SWAP OBJ\-> \->ARRY \-> c
b a
\<< a b
IF c 1 SAME
THEN OBJ\->
DROP / OBJ\-> 1 GET
\->LIST { 0 }
ELSE
WHILE
OVER SIZE 1 GET c \>=
REPEAT
DIVV
END DROP
\-> d
\<< a SIZE
1 GET c 1 - - \->LIST
d OBJ\-> OBJ\-> DROP
\->LIST
\>>
END
\>>
\>>
SYML
\<< 0 \-> E n
\<<
DO 0 'X'
STO E EVAL n ! /
'X' PURGE 1 'n'
STO+
UNTIL E 'X'
\.d DUP 'E' STO
IF TYPE 0
==
THEN
IF E 0
==
THEN 1
ELSE 0
END
ELSE 0
END
END 2 n
FOR i i
ROLL
NEXT n
\->LIST
\>>
\>>
LSYM
\<< 'X' \-> x
\<< LIST\-> 0
SWAP 1
FOR n n 1 +
ROLL x n 1 - ^ * +
-1
STEP
\>>
\>>
IRT
\<< OBJ\-> \-> n
\<<
IF n 0 >
THEN 1 n
START n
ROLL { 1 } SWAP NEG
+
NEXT
ELSE { 1 }
END
IF n 1 >
THEN 2 n
START
PMUL
NEXT
END
\>>
\>>
MULTI
\<< \-> p
\<<
DO DUP p
EVAL
UNTIL DUP
ROT SAME
END
\>>
\>>
PROOT
\<< LIST\-> \->ARRY
DUP 1 GET / ARRY\->
LIST\-> DROP 1 - DUP
2 + ROLL DROP { NEG
QUD CUBIC QUAR }
SWAP GET EVAL
\>>
QUAR
\<< 3 PICK NEG
DUP 6 ROLLD 5 PICK
4 PICK * 3 PICK 4 *
- 5 ROLL 4 * 6 PICK
SQ - 4 PICK * 5
PICK SQ - CUBIC 3
\->LIST 1 3
FOR n DUP n
GET 2 / SQ n 2 +
PICK - ABS SWAP
NEXT 4 ROLLD
DUP2
IF <
THEN SWAP
DROP 3
ELSE DROP 2
END 3 ROLLD
IF >
THEN DROP 1
END GET 4
ROLL 2 / SWAP 2 /
DUP SQ 4 ROLL - \v/
IF DUP ABS
THEN 3 DUPN 3
ROLLD * 6 ROLL 2 /
- SWAP /
ELSE 3 PICK
SQ 6 ROLL + 3 PICK
2 * + \v/
END 5 ROLL
DROP 3 ROLLD 4 DUPN
+ 3 ROLLD + SWAP
QUD 6 ROLLD 6 ROLLD
- 3 ROLLD - SWAP
QUD
\>>
CUBIC
\<< 3 PICK -3 / 3
PICK 5 PICK SQ 3 /
- 5 ROLL DUP 3 ^ 2
* SWAP 9 * 6 ROLL *
- 27 / 4 ROLL + NEG
OVER ABS 0
IF ==
THEN 3 INV ^
SWAP DROP 0 SWAP
ELSE 2 / DUP
SQ 3 PICK 3 ^ 27 /
+ \v/ - 3 INV ^ SWAP
OVER / 3 / NEG
END -1 3 \v/
R\->C 2 / 4 ROLLD 3
DUPN 3 DUPN + + 8
ROLLD 7 PICK * SWAP
7 PICK / + + 5
ROLLD 4 PICK / SWAP
4 ROLL * + +
\>>
TRIM
\<< OBJ\-> \-> n
\<< n
WHILE ROLL
DUP NOT n 1 - AND
REPEAT DROP
'n' DECR
END n ROLLD
n \->LIST
\>>
\>>
RDER
\<< \-> F G
\<< G F PDER
PMUL G PDER { -1 }
PMUL F PMUL PADD G
G PMUL
\>>
\>>
PDER
\<< OBJ\-> \-> n
\<< 1 n
FOR i n
ROLL n i - *
NEXT DROP
IF n 1 ==
THEN { 0 }
ELSE n 1 -
\->LIST
END
\>>
\>>
PF
\<< MAXR { } \-> Z
P OLD LAST
\<< 1 P SIZE
FOR I P I
GET \-> p1
\<<
IF p1
OLD \=/
THEN Z
p1 EVPLY 1 P SIZE
FOR J
IF P J GET P I GET
\=/
THEN p1 P J GET - /
END
NEXT
p1 'OLD' STO { }
'LAST' STO
ELSE
IF {
} LAST SAME
THEN
1 { } 1 P SIZE
FOR K P K GET
IF DUP p1 ==
THEN DROP
ELSE +
END
NEXT IRT Z SWAP
ELSE
LAST OBJ\-> DROP
END
RDER DUP2 5 PICK 1
+ 3 ROLLD 3 \->LIST
'LAST' STO p1 EVPLY
SWAP p1 EVPLY SWAP
/ SWAP ! /
END
\>>
NEXT P SIZE
\->LIST
\>>
\>>
FCTP
\<<
IF DUP SIZE 3
>
THEN DUP
BAIRS SWAP OVER
PDIV DROP FCTP
END
\>>
L\178A
\<<
IF DUP TYPE 5
==
THEN OBJ\->
\->ARRY
ELSE OBJ\-> 1
GET \->LIST
END
\>>
EVPLY
\<< OVER
IF DUP TYPE 5
==
THEN SIZE
ELSE SIZE 1
GET
END \-> a r n
\<< a 1 GET
IF n 1 >
THEN 2 n
FOR i r *
a i GET +
NEXT
END
\>>
\>>
COEF
\<< \-> E n
\<< 0 n
FOR I 0 'X'
STO E EVAL 'X'
PURGE E 'X' \.d 'E'
STO I ! /
NEXT 2 n 1
+
FOR I I
ROLL
NEXT n 1 +
\->LIST
\>>
\>>
EQ 1
DIVV
\<< DUP 1 GET \-> a
b c
\<< a 1 GET c /
DUP b * a SIZE RDM
a SWAP - OBJ\-> 1
GETI 1 - PUT \->ARRY
SWAP DROP b
\>>
\>>
QUD
\<< SWAP 2 / NEG
DUP SQ ROT - \v/ DUP2
+ 3 ROLLD -
\>>
BAIRS
\<< OBJ\-> 1 1 \-> n
R S
\<<
DO 0 n 1 +
PICK 0 0 0 4 PICK 5
n + 7
FOR J J
PICK R 7 PICK * + S
8 PICK * + 7 ROLL
DROP DUP 6 ROLLD R
3 PICK * + S 4 PICK
* + 5 ROLL DROP -1
STEP 3
PICK SQ 3 PICK 6
PICK * -
IF DUP 0
==
THEN DROP
1 1
ELSE 6
PICK 6 PICK * 5
PICK 9 PICK * -
OVER / 4 PICK 9
PICK * 8 PICK 7
PICK * - ROT /
END DUP
'S' STO+ SWAP DUP
'R' STO+
UNTIL R\->C
ABS .000000001 < 7
ROLLD 6 DROPN
END n DROPN
1 R NEG S NEG 3
\->LIST
\>>
\>>
CST { UP EXCO
ROOTS PADD PMUL
PDIV SYML LSYM IRT
}
END @ END OF PG3 DIRECTORY
