Groups | Search | Server Info | Keyboard shortcuts | Login | Register [http] [https] [nntp] [nntps]
Groups > comp.lang.forth > #12703
| From | mhx@iae.nl (Marcel Hendrix) |
|---|---|
| Subject | evplace (eigenvalue placement) |
| Newsgroups | comp.lang.forth |
| Message-ID | <56561599978435@frunobulax.edu> (permalink) |
| Date | 2012-06-04 20:47 +0200 |
| Organization | Wanadoo |
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 *)
Back to comp.lang.forth | Previous | Next — Next in thread | Find similar | Unroll thread
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
csiph-web