evplace (eigenvalue placement)

[Marcel Hendrix]

I wanted to compute a state-feedback matrix K such that
the eigenvalues of Phi-Gamma*K are those specified in a vector P.
Phi and Gamma are defined as dx/dt = Phi*x + Gamma*u, with x the state
variables and u the inputs of some system.

The subproblem that caused me some unexpected headache was
calculation of the polynomial coefficients of p(z) when given the
matrix of eigenvalues P. (Either real or complex conjugated pairs.)

The definition of p(z) = (z-p1)*(z-p2) ... (z-pn), with pi the
eigenvalues stored in the vector P.
E.g. when P = [0.7784+0i, 0.8055+0.1543i, 0.8055-0.1543i]
p(z) = z^3 - 2.3894 z^2 + 1.926641 z - 0.523582.

Having evplace, obsv, and ctrb means one doesn't need access to Matlab
for the design of simple digital controllers.

-marcel

-- ---------------------------------------------------------
(*
* LANGUAGE : ANS Forth with extensions
* PROJECT : Forth Environments
* DESCRIPTION : Closed-loop pole assignment using state feedback.
* CATEGORY : Tool to design digital controllers
* AUTHOR : Marcel Hendrix
* LAST CHANGE : Monday, June 04, 2012, 20:26, Marcel Hendrix; documentation, temp values
* LAST CHANGE : June 2, 2012, Marcel Hendrix
*)



NEEDS -miscutil
NEEDS -gaussj

REVISION -evplace "--- EVPLACE Version 0.01 ---"

PRIVATES

DOC
(*
evplace ( A{ B{ P{ K{ -- ) computes a state-feedback matrix K such that
the eigenvalues of A-B*K are those specified in the vector P.

Ref:: Kautsky, Nichols, Van Dooren, "Robust Pole Assignment in Linear
State Feedback," Intl. J. Control, 41(1985)5, pp 1129-1155
*)
ENDDOC

COMPLEX DARRAY ev{ PRIVATE -- helper for eigen
DOUBLE DARRAY coeff{ PRIVATE -- desired poles (polynomial form)
DOUBLE DARRAY cev{ PRIVATE -- open-loop poles (polynomial form)
DOUBLE DARRAY Kk{ PRIVATE -- temporary return for =evplace

DOUBLE DMATRIX obsv{{ PRIVATE -- temporary return for obsv
DOUBLE DMATRIX ctrb{{ PRIVATE -- temporary return for ctrb
DOUBLE DMATRIX Wc_bar{{ PRIVATE -- internal, temp. return
DOUBLE DMATRIX Tc{{ PRIVATE -- internal, temp. return

0 VALUE Phi{{ PRIVATE -- Four input pointers
0 VALUE Gamma{{ PRIVATE
0 VALUE p{ PRIVATE
0 VALUE k{ PRIVATE

0 VALUE nx PRIVATE -- rows of Phi
0 VALUE na PRIVATE -- columns of Phi
0 VALUE n PRIVATE -- rows of Gamma
0 VALUE m PRIVATE -- columns of Gamma

-- Computes the observability matrix Wo as an anonymous matrix. Standalone.
-- I am not sure this works for a c{{ with multiple rows.
: obsv ( phi{{ c{{ obsv{{ -- )
2 PICK CDIM 2 PICK RDIM LOCALS| n m obsv{{ c{{ phi{{ |
obsv{{ m n * m }}malloc
n m DOUBLE LMATRIX cPhi^i{{
c{{ cPhi^i{{ =>
cPhi^i{{ 0 obsv{{ 0 move-row
m n * n ?DO cPhi^i{{ phi{{ cPhi^i{{ mat*
cPhi^i{{ 0 n 1- 0 m 1- obsv{{ I 0 matmove
n +LOOP
cPhi^i{{ }}free ; PRIVATE

-- Computes the controllability matrix Wc as an anonymous matrix. Standalone.
-- I am not sure this works for a Gamma{{ with multiple columns.
: ctrb ( Phi{{ Gamma{{ ctrb{{ -- )
OVER DIMS LOCALS| n m ctrb{{ Gamma{{ Phi{{ |
ctrb{{ m m n * }}malloc
m n DOUBLE LMATRIX Phi^i*Gamma{{
Gamma{{ Phi^i*Gamma{{ =>
Phi^i*Gamma{{ 0 m 1- 0 n 1- ctrb{{ 0 0 matmove
m n * n ?DO Phi{{ Phi^i*Gamma{{ Phi^i*Gamma{{ mat*
Phi^i*Gamma{{ 0 m 1- 0 n 1- ctrb{{ 0 I matmove
n +LOOP
Phi^i*Gamma{{ }}free ; PRIVATE

-- Wc_bar = controllability matrix of
-- Phi_bar = [ a[z](0) a[z](1) ... \ nx x nx
-- 1 0 ...
-- 0 1 ...
-- .. .. ... ],
-- Gamma_bar = [1; 0; ... ]; ( nx rows )
: Wc_bar ( -- x{{ )
m 1 <> ABORT" Wc_bar :: don't know how to handle multi-column Gamma{{"
nx nx DOUBLE LMATRIX Phi{{
nx 1 DOUBLE LMATRIX Gamma{
nx 0 ?DO cev{ I } DF@ FNEGATE Phi{{ 0 I }} DF! LOOP 1e Gamma{ 0 } DF!
nx 1 ?DO 1e Phi{{ I I 1- }} DF! LOOP
Phi{{ Gamma{ Wc_bar{{ ctrb Wc_bar{{
Phi{{ }}free
Gamma{ }free ; PRIVATE

-- Tc = Wc_bar * inv(Wc)
: Tc ( -- x{{ ) Wc_bar Phi{{ Gamma{{ ctrb{{ ctrb ctrb{{ =mat^-1 Tc{{ mat* Tc{{ ; PRIVATE

DOC
(*
For n poles: p(z) = ( z - p1 ) ( z - p2 ) ... ( z - pn )
n = 1; p1(z) = z - p1 -> { 1 -p1 }
n = 2; p2(z) = (z-p2) p1(z) = z*p1(z) - p2 * p1(z) ->
{ 1 -p1 0 }
{ -p2 p1p2 } + ->
{ 1 -(p1+p2) p1p2 } = z^2 -(p1+p2) z + p1p2 -> { 1 -(p1+p2) p1p2 }
n = 3; p3(z) = (z-p3) p2(z) = z*p2(z) - p3 * p2(z) ->
{ 1 -(p1+p2) p1p2 0 }
{ 0 -p3 (p1+p2)p3 -p1p2p3 } + ->
{ 1 -(p1+p2+p3) p1p2+(p1+p2)p3 -p1p2p3 }

In words: For n poles we need an array of n real numbers
Place pn-1 in the first n-1 positions of pn
Add p * pn-1 to positions 1..n
*)
ENDDOC

: ev->coeffs ( in{ out{ -- )
LOCALS| out{ in{ |
0+0i ZLOCAL p
nx 1+ COMPLEX LARRAY zcoeff{
out{ nx }malloc
1+0i zcoeff{ 0 } Z! in{ 0 } Z@ ZNEGATE zcoeff{ 1 } Z!
nx 1 ?DO
in{ I } Z@ ZNEGATE TO p
1 nx DO zcoeff{ I 1- } Z@ p Z* zcoeff{ I } Z+! -1 +LOOP
LOOP
nx 0 ?DO zcoeff{ I 1+ } Z@ FABS eps F> ABORT" ev->coeffs :: insufficient precision" out{ I } DF! LOOP
zcoeff{ }free ; PRIVATE

: p->coeffs ( -- ) p{ coeff{ ev->coeffs ; PRIVATE
: eigen->coeffs ( -- ) Phi{{ ev{ eigen ev{ cev{ ev->coeffs ; PRIVATE

: Lbar*Tc->K ( -- )
nx DOUBLE LARRAY Lbar{
nx 0 DO coeff{ I } DF@ cev{ I } DF@ F- Lbar{ I } DF! LOOP
Lbar{ Tc K{ mat*
Lbar{ }free ; PRIVATE

: set-sizes? ( Phi{ Gamma{ P{ K{ -- )
TO k{ TO p{ TO Gamma{{ TO Phi{{
Phi{{ DIMS TO na TO nx
Gamma{{ DIMS TO m TO n
na nx <> ABORT" Phi should be square"
nx n <> ABORT" Phi and Gamma should have the same number of rows"
p{ DIMS MAX nx <> ABORT" P and Phi should have the same number of columns"
Phi{{ MTYPE DOUBLE D<>
Gamma{{ MTYPE DOUBLE D<> OR ABORT" Phi and Gamma can only be of type DOUBLE" ; PRIVATE

: evplace ( Phi{ Gamma{ P{ K{ -- ) set-sizes? p->coeffs eigen->coeffs Lbar*Tc->K ; \ Phi,Gamma,k real, p complex
: =evplace ( Phi{ Gamma{ P{ -- K_anon{ ) Kk{ evplace Kk{ ;

NESTING 1
= [IF]

:ABOUT CR ." ( Phi{ Gamma{ p{ k{ -- ) EVPLACE -- Closed-loop pole assignment using state feedback." ;

.ABOUT -evplace CR
DEPRIVE
[ELSE]

-- Given roots 0.7784, 0.8055 +/- 0.1543i,
-- from p(z) = ( z - 0.7784 ) * ( z - {0.8055 + 0.1543i} ) * ( z - {0.8055 - 0.1543i} ),
-- it follows p(z) = z^3 - 2.3893 z^2 + 1.9265 z - 0.5235

-- The eigenvalues of at{{ are 1, 0.9048 and 0.6703 => p(z) = z^3 - 2.5752 z^2 + 2.1817 z - 0.6065
3 3 DOUBLE MATRIX Phi_x{{
Phi_x{{ 3 3 }}fread
1 0.0952 0.0042
0 0.9048 0.0782
0 0 0.6703

3 1 DOUBLE MATRIX Gamma_x{ 0.0001e 0.0042e 0.0824e Gamma_x{ #=>
3 COMPLEX ARRAY p_x{ 0.7784e 0e 0.8055e 0.1543e 0.8055e -0.1543e p_x{ #=>
3 DOUBLE ARRAY k_x{
3 DOUBLE ARRAY c_x{ 1e 0e 0e c_x{ #=>

Phi_x{{ Gamma_x{ p_x{ k_x{ EVPLACE
CR .~ ev{ should be ( 1 0 ) ( 0.9048 0 ) ( 0.6703 0 ) ~ ev{ }print
CR .( cev{ should be [ -2.575100e+0000 2.181587e+0000 -6.064874e-0001 ] ) cev{ }print
CR .~ p{ should be ( 0.7784 0 ) ( 0.8055 0.1543 ) ( 0.8055 -0.1543 ) ~ p{ }print
CR .( coeff{ should be [ -2.389400e+0000 1.926641e+0000 -5.235820e-0001 ] ) coeff{ }print
CR
CR .( After Phi_x{{ Gamma_x{ ctrb{{ ctrb , ctrb{{ should be )
CR .( 1.000000e-0004 8.459200e-0004 2.053111e-0003)
CR .( 4.200000e-0003 1.024384e-0002 1.358783e-0002)
CR .( 8.240000e-0002 5.523272e-0002 3.702249e-0002 ; and is )
CR Phi_x{{ Gamma_x{ ctrb{{ ctrb ctrb{{ }}print

CR .( Wc_bar should be )
CR .( 1.000000e+0000 2.575100e+0000 4.449553e+0000)
CR .( 0.000000e+0000 1.000000e+0000 2.575100e+0000)
CR .( 0.000000e+0000 0.000000e+0000 1.000000e+0000 ; and is )
CR Wc_bar }}print

CR .~ Tc = Wc_bar * inv(Wc) should be ~
CR .( 1.279834e+0003 1.222239e+0002 4.352867e+0000)
CR .( 1.279834e+0003 4.240187e-0001 -1.574810e+0000)
CR .( 1.279834e+0003 -1.341912e+0002 5.286646e+0000 ; and is )
CR Tc }}print
CR
CR .( K{ should be [ 17.4134 11.4013 1.6358 ] ) K{ }print

Phi_x{{ 3 3 }}fread
1 0.0952 0.0042
0 0.9048 0.0782
0 0 0.6703

1e 0e 0e c_x{ #=>

CR
CR .( obsv should be )
CR .( 1 0 0 )
CR .( 1 0.0952 0.0042 )
CR .( 1 0.1813 0.0145 ; and is )
CR Phi_x{{ c_x{ obsv{{ obsv obsv{{ }}print

[THEN]

(* End of Source *)

[Krishna Myneni]

On Jun 4, 1:47 pm, m...@iae.nl (Marcel Hendrix) wrote:
> I wanted to compute a state-feedback matrix K such that
> the eigenvalues of Phi-Gamma*K are those specified in a vector P.
> Phi and Gamma are defined as dx/dt = Phi*x + Gamma*u, with x the state
> variables and u the inputs of some system.
>
> The subproblem that caused me some unexpected headache was
> calculation of the polynomial coefficients of p(z) when given the
> matrix of eigenvalues P. (Either real or complex conjugated pairs.)
>

Is this the ev->coeffs word that caused you a problem? Does the
present code work for the general case?

> The definition of p(z) = (z-p1)*(z-p2) ... (z-pn), with pi the
> eigenvalues stored in the vector P.
> E.g. when P = [0.7784+0i, 0.8055+0.1543i, 0.8055-0.1543i]
> p(z) = z^3 - 2.3894 z^2 + 1.926641 z - 0.523582.
>
> Having evplace, obsv, and ctrb means one doesn't need access to Matlab
> for the design of simple digital controllers.
>

Does it apply as well to continuous analog controllers, or does your
implementation assume some sort of discretization?

Krishna

[Arnold Doray]

On Mon, 04 Jun 2012 20:47:47 +0200, Marcel Hendrix wrote:

> The subproblem that caused me some unexpected headache was calculation
> of the polynomial coefficients of p(z) when given the matrix of
> eigenvalues P. (Either real or complex conjugated pairs.)
>
> The definition of p(z) = (z-p1)*(z-p2) ... (z-pn), with pi the
> eigenvalues stored in the vector P.
> E.g. when P = [0.7784+0i, 0.8055+0.1543i, 0.8055-0.1543i]
> p(z) = z^3 - 2.3894 z^2 + 1.926641 z - 0.523582.

Here's a solution using Higher Order Functions with Lazy Evaluation.
The trick is to correctly represent the polynomial as an lazy infinite
list.

\ Multiply a polynomial by ( Z - c )
: *(z-c) { c } ( poly c -- poly' )
dup [: c -1 * * ;] map 0 cons \ multiply by -c and demote the poly.
['] + zipwith ;

\ Calculates the characteristic polynomial with N factored terms
: char-poly ( c1 c2..cN N -- poly )
0 0 ... 1 cons \ this is just 1,0,0...
swap dup 0 do
>R swap *(z-c) R>
loop
1+ take ;

0.7784
0.8055::0.1543 \ ie, 0.8055 + 0.1543i in my Forth.
0.8055::-0.1543
3

char-poly .list

1
{-2.38939999999999996838084825867554172873497009277343750 , 0E-53i}
{1.926641139999999947150968182540964665451900138091569717566046569985
6271903911419940413907170295715332031250 , 0E-106i}
{-0.52358199521599997754971805363766210317773486756087243684688815687
00625016519242871603082500336281690305497623505329510273931248320877784863114356994628906250 ,
0E-157i} ok

Cheers,
Arnold

[Marcel Hendrix]

Krishna Myneni <krishna...@ccreweb.org> writes Re: evplace (eigenvalue placement)

> On Jun 4, 1:47 pm, m...@iae.nl (Marcel Hendrix) wrote:

[..]

> Is this the ev->coeffs word that caused you a problem?

> Does the
> present code work for the general case?

In my application p(z) will always have real coefficients. This is
equivalent to saying that the poles p1,..pn are either real or appear
in complex conjugated pairs. Arbitrary isolated complex poles don't
happen in electrical networks. I use this fact to make the type of
p(z) DOUBLE instead of COMPLEX. However, with a slight change the
code should work for complex coefficients, too (and be even simpler).

[..]

>> Having evplace, obsv, and ctrb means one doesn't need access to Matlab
>> for the design of simple digital controllers.

> Does it apply as well to continuous analog controllers, or does your
> implementation assume some sort of discretization?

I use this for digitally controlled power electronics circuitry.
The code that I've shown only works when an analog circuits is
sampled, i.e. dx/dt = A x + B u should be converted to x[k+1] =
Phi x[k] + Gamma u[k]. This can be done by using the EXPM function
(exponential matrix) and Van Loan's famous formula to prevent
integrating the B u term. The pi are poles in the Z domain, and
for a stable system they have the property that zabs(pi) <= 1.
Maybe this can be used to further simplify the code.

An SMPS is inherently a discrete circuit.

-marcel