%%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