Groups | Search | Server Info | Keyboard shortcuts | Login | Register [http] [https] [nntp] [nntps]

Groups > comp.lang.forth > #12703 > unrolled thread

evplace (eigenvalue placement)

Started bymhx@iae.nl (Marcel Hendrix)
First post2012-06-04 20:47 +0200
Last post2012-06-05 16:15 +0000
Articles 4 — 3 participants

Back to article view | Back to comp.lang.forth

Contents

  evplace (eigenvalue placement) mhx@iae.nl (Marcel Hendrix) - 2012-06-04 20:47 +0200
    Re: evplace (eigenvalue placement) Krishna Myneni <krishna.myneni@ccreweb.org> - 2012-06-04 21:08 -0700
      Re: evplace (eigenvalue placement) mhx@iae.nl (Marcel Hendrix) - 2012-06-05 20:39 +0200
    Re: evplace (eigenvalue placement) Arnold Doray <invalid@invalid.com> - 2012-06-05 16:15 +0000

#12703 — evplace (eigenvalue placement)

Frommhx@iae.nl (Marcel Hendrix)
Date2012-06-04 20:47 +0200
Subjectevplace (eigenvalue placement)
Message-ID<56561599978435@frunobulax.edu>
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 *)

[toc] | [next] | [standalone]

#12708

FromKrishna Myneni <krishna.myneni@ccreweb.org>
Date2012-06-04 21:08 -0700
Message-ID<a7c45f39-a778-4c4f-b653-326b1af1c3b2@w24g2000vby.googlegroups.com>
In reply to#12703
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

[toc] | [prev] | [next] | [standalone]

#12720

Frommhx@iae.nl (Marcel Hendrix)
Date2012-06-05 20:39 +0200
Message-ID<00641598978435@frunobulax.edu>
In reply to#12708
Krishna Myneni <krishna.myneni@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

[toc] | [prev] | [next] | [standalone]

#12717

FromArnold Doray <invalid@invalid.com>
Date2012-06-05 16:15 +0000
Message-ID<jqlbad$alr$1@dont-email.me>
In reply to#12703
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

[toc] | [prev] | [standalone]

Back to top | Article view | comp.lang.forth

csiph-web