EKF y UKF: dos extensiones del filtro de

Kalman para sistemas no lineales aplicadas al

control de un pendulo invertido

Alejandro Pascual

8 de mayo de 2006

Monografıa para el curso Tratamiento Estadıstico de Senales (2004).Docentes: Gabriel Dutra, Gabriel Perret y Alvaro Tuzman.Instituto de Ingenierıa Electrica - Facultad de Ingenierıa - UDELAR .

Indice

1. Introduccion 1

2. Estimacion de estado optima 22.1. Formulacion del problema . . . . . . . . . . . . . . . . . . . . . . 22.2. Filtro de Kalman . . . . . . . . . . . . . . . . . . . . . . . . . . . 42.3. Filtro de Kalman extendido (EKF) . . . . . . . . . . . . . . . . . 52.4. La transformacion “unscented” . . . . . . . . . . . . . . . . . . . 7

2.4.1. Ejemplo . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92.5. Filtro de Kalman “unscented” (UKF) . . . . . . . . . . . . . . . 11

3. Aplicacion 133.1. La planta a controlar . . . . . . . . . . . . . . . . . . . . . . . . . 13

3.1.1. Dinamica de la planta en tiempo continuo . . . . . . . . . 143.1.2. El objetivo de control . . . . . . . . . . . . . . . . . . . . 153.1.3. Dinamica de la planta en tiempo discreto . . . . . . . . . 16

3.2. Modelado . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 173.3. Algoritmo de control . . . . . . . . . . . . . . . . . . . . . . . . . 193.4. Observador de estado . . . . . . . . . . . . . . . . . . . . . . . . 223.5. Resultados de las simulaciones . . . . . . . . . . . . . . . . . . . 23

3.5.1. Sintonizacion en lazo abierto . . . . . . . . . . . . . . . . 243.5.2. Estimacion de estado en lazo cerrado . . . . . . . . . . . . 26

4. Conclusiones 32

1. Introduccion

El filtro de Kalman, aparecido a principios de la decada de los sesenta, es deuna importancia comparable a los trabajos realizados por Nyquist y Bode en

1

la decada de los veinte y los de Wiener en los anos treinta. El filtro de Kalmanpermite estimar en tiempo real el vector de estado de un sistema dinamicolineal a partir de medidas ruidosas indirectas que de el se van tomando. Estasestimaciones en tiempo real del estado del sistema, son valiosas en si mismascuando el sistema opera en lazo abierto; pero considerando la posibilidad deoperacion en lazo cerrado, las mismas pueden ser utilizadas para sintetizar unaaccion de control adecuada que lleve el sistema al estado deseado. Esta ultimaposibilidad es la que motiva nuestro interes en el filtro.

En el contexto de la teorıa de control lineal, el filtro de Kalman ocupa unlugar central como observador de estado optimo y en este contexto esta bienentendida su utilizacion. No obstante, cada vez con mas frecuencia, se manejanmodelos no-lineales en ambitos de aplicacion tan diversos como la ingenierıa,biologıa, ecologıa y economıa. El uso de estos modelos mas complejos, se havisto estimulado por el desarrollo tecnologico de la computacion, que medianteel calculo numerico ha facilitado el analisis de los mismos y la implementacion demetodos de control tambien no-lineales. Resulta entonces natural buscar algunmetodo de estimacion de estado en tiempo real, que pueda aplicarse a sistemasno-lineales.

En este trabajo se presentan dos extensiones del filtro de Kalman para sis-temas no lineales: 1) el filtro de Kalman extendido (EKF: Extended KalmanFilter), utilizado hasta hace poco como la opcion estandar en aplicaciones no-lineales y 2) el filtro de Kalman “unscented” 1 (UKF: Unscented Kalman Fil-ter), propuesto recientemente por Julier y Uhlmann [4] y luego perfeccionadopor Wan y van der Merwe [8].

Estas dos extensiones del filtro de Kalman, se aplican luego como observado-res de estado, en un problema clasico de control no-lineal conocido como “controldel pendulo invertido” o “problema del escobillero”; que consiste en mantenerun pendulo verticalmente, en su posicion naturalmente inestable, aplicando, encada instante, una fuerza horizontal apropiada a un carro deslizante sobre el quese encuentra montado el eje del pendulo. Utilizando una misma metodologıa decontrol no-lineal, se comparan los desempenos de los dos observadores en sufuncion de proveer estimaciones del vector de estado para la sıntesis de la senalde control.

2. Estimacion de estado optima

En esta seccion se plantea detalladamente el problema de estimacion deestado, se revisa la formulacion del filtro de Kalman para tiempo discreto y seintroducen el EKF y el UKF.

2.1. Formulacion del problema

Matematicamente, el problema a abordar es el siguiente. Se cuenta con unmodelo de la planta en la forma de una representacion en variables de estadoen tiempo discreto:

xk = F (xk−1, uk−1, vk−1) (1)

1“Unscented” podrıa traducirse literalmente como inoloro, aunque hemos preferido man-tener el nombre original en ingles porque desconocemos los motivos que los autores tuvieronpara bautizar de esta forma al nuevo filtro.

2

yk = G (xk, nk) (2)

donde los subındices hacen referencia a los instantes de tiempo discreto, la va-riable aleatoria xk representa el estado no accesible del sistema, uk es la entradaimpuesta al sistema (supuesta conocida) e yk es la salida observada. Los pro-cesos estocasticos {vk}k∈N y {nk}k∈N, son habitualmente referidos como ruidode proceso y ruido de medida respectivamente. El ruido de proceso afecta ladinamica del modelo y representa los aspectos no modelados de la dinamica dela planta real; mientras que el ruido de medida influye sobre la salida accesibledel modelo y representa la incertidumbre propia del proceso de medida de la sa-lida de la planta real. Tanto el ruido de proceso como el ruido de medida puedenademas representar perturbaciones corrientes a las que esta sometida la planta.Se asume que la condicion inicial x0 desconocida, vk y nk son mutuamente nocorrelacionadas:

Px0,vk, 2Cov (x0, vk) = 0 ∀k ≥ 0 (3)

Px0,nk, Cov (x0, nk) = 0 ∀k ≥ 0 (4)

Pvk,nj , Cov (vk, nj) = 0 ∀k, j ≥ 0 (5)

y que {vk} y {nk} son ruido blanco:

Pvk,vj , Cov (vk, vj) = δ (k − j)Q ∀k, j ≥ 0 (6)

Pnk,nj , Cov (nk, nj) = δ (k − j) R ∀k, j ≥ 0 (7)

donde δ (k) es la delta de Kronecker 3 y Q y R son las matrices de covarianzadel ruido de proceso y el ruido de medida, respectivamente.

Bajo estas hipotesis el estado xk y la salida yk son variables aleatorias.Dadas las estadısticas de los procesos estocasticos {vk} y {nk}, la estadısticade la condicion inicial desconocida x0, la sucesion de entrada conocida u0:k ={u0, . . . , uk} y las observaciones y1:k = {y1, . . . , yk}; el objetivo es estimar, encada instante k ≥ 1, el estado xk.

La estimacion xk|k de xk es optima en el sentido “mınima media cuadraticadel error” (MMSE: minimum mean-squared error) si es aquella que minimiza

J = E[(

xk − xk|k)T (

xk − xk|k) | y1:k

]. (8)

La estimacion optima viene dada por la esperanza condicional (Lewis [5]):

xMMSEk|k = E [xk | y1:k] =

∫xkp (xk | y1:k) dxk . (9)

La evaluacion de (9) requiere del conocimiento de la densidad de probabilidada posteriori p (xk | y1:k) 4. Dada esta densidad, es posible determinar no soloel estimador MMSE que minimiza (8), sino cualquier otro estimador optimoen el sentido de algun criterio de performance. El problema de determinar esta

2Cov (x, y) , Eh(x− E [x]) (y − E [y])T

i(matriz de covarianza).

3δ (k) = 1 si k = 0 y δ (k) = 0 si k 6= 0.4No se hace explıcita la dependencia con la entrada {uk} porque se asume que es una senal

conocida y no un proceso estocastico.

3

densidad es referido como enfoque Bayesiano y la misma puede ser evaluadarecursivamente mediante (Lewis [5]):

p (xk | y1:k) =p (xk | y1:k−1) p (yk | xk)

p (yk | y1:k−1)(10)

dondep (xk | y1:k−1) =

∫p (xk | xk−1) p (xk−1 | y1:k−1) dxk−1 (11)

y la constante de normalizacion p (yk | y1:k−1) esta dada por:

p (yk | y1:k−1) =∫

p (xk | y1:k−1) p (yk | xk) dxk . (12)

Esta recursion permite calcular la densidad de probabilidad del estado actualen funcion de la densidad previa y la medida mas reciente. p (xk | xk−1) sedetermina a partir de p (vk), la ecuacion de transicion de estado (1) y la entradauk que es conocida. En forma similar, p (yk | xk) se determina a partir de p (nk)y la ecuacion de medida (2).

En principio, el conocimiento de estas densidades y p (x0 | y1:0) , p (x0)determina p (xk | y1:k) para todo k ≥ 1. Desafortunadamente, para la mayorıade los modelos que surgen en la practica, no es posible encontrar una solucioncerrada a esta recursion. El unico abordaje practico existente, de tipo com-pletamente general, consiste en aplicar tecnicas de muestreo Monte-Carlo, queesencialmente aproximan las integrales de las ecuaciones (11) y (12) por sumasque convergen a la solucion en el lımite, con considerable costo de procesamientonumerico. Solo introduciendo fuertes hipotesis, el problema se vuelve tratableanalıticamente. El celebre filtro de Kalman que se presenta a continuacion, esun ejemplo de ello.

2.2. Filtro de Kalman

La esperanza condicional (9) es en general muy difıcil de calcular porque im-plica conocer p (xk | y1:k), pero en algunos casos puede calcularse analıticamenteen forma recursiva.

Considerese el caso en que el modelo (1),(2) es lineal:

xk = Axk−1 + uk−1 + vk−1 (13)

yk = Cxk + nk (14)

y las densidades de distribucion son gaussianas:

p (xk−1 | y1:k−1) = N (xk−1|k−1, Pk−1|k−1

)(15)

p (vk−1) = N (0, Q) (16)

p (nk) = N (0, R) (17)

donde N (x, Px) es una densidad de distribucion gaussiana caracterizada por elvector esperanza x y la matriz de covarianza Px.

En estas condiciones se tiene que,

p (xk | y1:k−1) = N (xk|k−1, Pk|k−1

)(18)

4

p (xk | y1:k) = N (xk|k, Pk|k

)(19)

dondexk|k−1 = Axk−1|k−1 + uk−1 = E [xk | y1:k−1] (20)

Pk|k−1 = APk−1|k−1AT + Q = Cov (xk, xk | y1:k−1) (21)

xk|k = xk|k−1 + PxkykP−1

ykyk

(yk − yk|k−1

)= E [xk | y1:k] (22)

Pk|k = Pk|k−1 − PxkykP−1

ykykPT

xkyk= Cov (xk, xk | y1:k) (23)

conyk|k−1 = Cxk|k−1 = E [yk | y1:k−1] (24)

Pxkyk= Pk|k−1C

T = Cov (xk, yk | y1:k−1) (25)

Pykyk= CPk|k−1C

T + R = Cov (yk, yk | y1:k−1) . (26)

Como p (xk−1 | y1:k−1), p (vk−1) y p (nk) son densidades de distribucionesgaussianas y como el modelo (modelo dinamico (13) y modelo de medida (14))es lineal; la densidad de distribucion p (xk | y1:k) tambien es gaussiana y porlo tanto queda completamente caracterizada por el vector esperanza xk|k y lamatriz de covarianza Pk|k.

Si la condicion inicial desconocida tiene una densidad de distribucion gaus-siana: p (x0 | y1:0) , p (x0) = N (

x0|0, P0|0)

y para todo k ≥ 0 los ruidos (de pro-ceso y de medida) tambien son gaussianos: p (vk) = N (0, Q), p (nk) = N (0, R);entonces es posible aplicar la recursion anterior, conocida como filtro de Kalman,para calcular en forma exacta los parametros xk|k y Pk|k que caracterizan a ladensidad de distribucion gaussiana p (xk | y1:k) con la cual se puede determinarcualquier estimador optimo segun algun criterio de performance. En particularel estimador MMSE que minimiza (8), es simplemente xMMSE

k|k = E [xk | y1:k] =xk|k.

Cuando p (x0), p (vk) o p (nk) no son gaussianas, el filtro de Kalman es elque minimiza (8) entre todos los estimadores lineales (Lewis [5]). Por supuesto,podrıan existir estimadores no-lineales que lograran reducir aun mas (8).

Cada paso de recursion del filtro de Kalman se compone de dos etapas:1) prediccion (o “actualizacion en el tiempo”, time update) y 2) correccion (o“actualizacion de medida”, measurement update). Las expresiones (20) y (21)constituyen la etapa de prediccion ya que con ellas se calculan las estimacionesa priori : xk|k−1 y Pk|k−1 propagando xk−1|k−1 y Pk−1|k−1 a traves del modelo.Las expresiones (22) y (23) constituyen la etapa de correccion ya que con ellas secorrigen las estimaciones a priori xk|k−1 y Pk|k−1 para generar las estimaciones aposteriori xk|k y Pk|k. El producto Pxkyk

P−1ykyk

en (22) se conoce como gananciade Kalman K, ya que pondera el impacto de la innovacion yk − yk|k−1 en laestimacion a posteriori del estado: xk|k = xk|k−1 + K

(yk − yk|k−1

).

2.3. Filtro de Kalman extendido (EKF)

El filtro de Kalman extendido (EKF: extended Kalman filter) consiste enuna variacion del filtro de Kalman para abordar el problema de estimacion delestado cuando el modelo es posiblemente no-lineal:

xk = F (xk−1, uk−1, vk−1) (27)

5

yk = G (xk, nk) . (28)

Al igual que para el filtro de Kalman se asume que:

p (xk−1 | y1:k−1) = N (xk−1|k−1, Pk−1|k−1

)(29)

p (vk−1) = N (0, Q) (30)

p (nk) = N (0, R) (31)

pero ahora en el instante siguiente k, las densidades de distribucion p (xk | y1:k−1)y p (xk | y1:k) no son necesariamente gaussianas ya que el modelo no es necesa-riamente lineal. No obstante, se aproximan estas densidades de distribucion pordensidades gaussianas:

p (xk | y1:k−1) ≈ N (xk|k−1, Pk|k−1

)(32)

p (xk | y1:k) ≈ N (xk|k, Pk|k

)(33)

donde:xk|k−1 = F

(xk−1|k−1, uk−1, 0

) ≈ E [xk | y1:k−1] (34)

Pk|k−1 = Ak−1Pk−1|k−1ATk−1 + Γk−1QΓT

k−1 ≈ Cov (xk, xk | y1:k−1) (35)

xk|k = xk|k−1 + PxkykP−1

ykyk

(yk − yk|k−1

) ≈ E [xk | y1:k] (36)

Pk|k = Pk|k−1 − PxkykP−1

ykykPT

xkyk≈ Cov (xk, xk | y1:k) (37)

conyk|k−1 = G

(xk|k−1, 0

) ≈ E [yk | y1:k−1] (38)

Pxkyk= Pk|k−1C

Tk ≈ Cov (xk, yk | y1:k−1) (39)

Pykyk= CkPk|k−1C

Tk + ΛkRΛT

k ≈ Cov (yk, yk | y1:k−1) (40)

y

Ak−1 =∂F

∂x

(xk−1|k−1, uk−1, 0

)(41)

Γk−1 =∂F

∂v

(xk−1|k−1, uk−1, 0

)(42)

Ck =∂G

∂x

(xk|k−1, 0

)(43)

Λk =∂G

∂n

(xk|k−1, 0

). (44)

Observese que estas expresiones son muy similares a las del filtro de Kalman, sal-vo por las aproximaciones que se indican explıcitamente. Estas aproximacionesserıan exactas solo si F y G fueran lineales.

En realidad, como el algoritmo es recursivo, el EKF aproxima la distribucionde probabilidad real del estado xk, en cada instante k, por una distribuciongaussiana y modifica el filtro de Kalman, de forma tal que:

el primer momento de la distribucion (supuesta gaussiana), se propaga atraves del modelo no-lineal,

el segundo momento de la distribucion (supuesta gaussiana), se propagaa traves de la version linealizada del modelo.

6

El EKF es entonces una forma de obtener aproximaciones “de primer orden”a los terminos optimos. Cuando el modelo es severamente no lineal, estas apro-ximaciones pueden pueden generar esperanzas y covarianzas muy distintas a lasesperanzas y covarianzas reales producto de la transformacion de la variablealeatoria a traves del modelo no lineal. Estas discrepancias, pueden conducir aun desempeno muy pobre del filtro o incluso a su divergencia.

Otro inconveniente del EKF es que requiere de las matrices jacobianas (41) a(44). El calculo de estas matrices no es trivial en la mayorıa de las aplicaciones,por lo que es muy factible cometer errores de calculo que luego son difıciles dedetectar. Ademas, el requerimiento de estas matrices complica la implementa-cion generica del filtro (no restringida a una aplicacion particular).

2.4. La transformacion “unscented”

La transformacion “unscented” (UT: unscented transformation) es un meto-do propuesto por Julier y Uhlmann [4] y luego perfeccionado por Wan y van derMerwe [8] para calcular los primeros momentos de la densidad de distribucionde probabilidad de una variable aleatoria y = f(x) que resulta de aplicar unatransformacion no-lineal f a una variable aleatoria x de estadıstica conocida.

El metodo se basa en la siguiente idea intuitiva:

“Resulta mas facil aproximar una distribucion gaussiana que apro-ximar una transformacion no-lineal arbitraria.” J. K. Uhlmann [7].

Un conjunto de puntos (puntos sigma, sigma points) se eligen de forma talque la media y la covarianza del conjunto de puntos coincida con la esperanzay covarianza de la variable aleatoria x. Luego, la transformacion no-lineal f seaplica a cada punto sigma, para generar una nube de puntos transformados. Dela estadıstica de los puntos transformados, se estima la esperanza y covarianzade la variable aleatoria y = f(x). Si bien el metodo es similar a los metodosde muestreo Monte Carlo, existe una diferencia fundamental que es que lasmuestras no se toman al azar, sino de acuerdo a un algoritmo determinista. Deesta forma se logra capturar informacion de alto orden de la distribucion deprobabilidad, tomando solo un numero pequeno de puntos.

Supongase que la variable aleatoria vectorial x, de dimension L, tiene espe-ranza E [x] = x (vector columna de L componentes) y covarianza Cov (x, x) = Px

(matriz L×L). Para calcular (aproximadamente) la esperanza E [y] = y y la co-varianza Cov (y, y) = Py de la variable aleatoria y = f (x) se comienza constru-yendo una matriz X de 2L+1 vectores columna Xi de acuerdo con lo siguiente:

X0 = x (45)

Xi = x +(√

(L + λ)Px

)i

i = 1, . . . , L (46)

Xi = x−(√

(L + λ)Px

)i−L

i = L + 1, . . . , 2L (47)

donde λ = α2 (L + κ)−L es un parametro de escala. La constante α determinala dispersion de los puntos sigma alrededor de x y habitualmente se elige iguala una pequena constante positiva (por ejemplo 1×10−4 ≤ α ≤ 1). La constanteκ es un parametro de escala secundario que habitualmente se toma igual a 0 oa 3− L (ver [3] por mas detalles) y β es usada para incorporar el conocimiento

7

que se tenga de antemano acerca de la distribucion de x (por ejemplo, para dis-tribuciones gaussiana, β = 2 es optimo).

(√(L + λ) Px

)ies la i-esima columna

de la matriz raız cuadrada 5 de (L + λ)Px.Es facil verificar que los puntos sigma capturan la esperanza y covarianza de

x, es decir:

x(UT ) ,2L∑

i=0

W(m)i Xi = x (48)

Px(UT ) ,

2L∑

i=0

W(c)i

(Xi − x(UT )

)(Xi − x(UT )

)T

= Px (49)

donde:W

(m)0 =

λ

L + λ(50)

W(c)0 =

λ

L + λ+ 1− α2 + β (51)

W(m)i = W

(c)i =

12 (L + λ)

i = 1, . . . , 2L . (52)

Luego, los puntos sigma son propagados a traves de la transformacion no-lineal:

Yi = f (Xi) i = 0, . . . , 2L (53)

y la esperanza y la covarianza de y se aproxima por las sumas ponderadas:

y ≈ y(UT ) =2L∑

i=0

W(m)i Yi (54)

Py ≈ Py(UT ) =

2L∑

i=0

W(c)i

(Yi − y(UT )

)(Yi − y(UT )

)T

(55)

con los pesos W(m)i y W

(c)i definidos igual que antes.

Este metodo genera estimaciones de y y Py correctas hasta el tercer ordende la esperanza y covarianza del desarrollo multivariable de Taylor de la fun-cion f (supuesta analıtica en todo valor posible de x) en torno a x, cuandola distribucion de x es simetrica 6 (gaussiana o student-T por ejemplo). Paradistribuciones no simetricas, genera estimaciones correctas hasta el segundo or-den, dependiendo la exactitud de los momentos de tercer y mayor orden, de laeleccion de los parametros α y β (Wan & van der Merwe [8]).

5Si A es simetrica, R =√

A es cualquier matriz que verifique RRT = A. La descomposicionde Cholesky consiste en descomponer la matriz A en la forma A = UT U siendo U una matriztriangular superior; ası que R = UT es una forma numericamente eficiente de calcular R =

√A.

6Si una distribucion es simetrica, entonces se puede verificar que sus momentos imparesson nulos.

8

2.4.1. Ejemplo

Julier y Uhlmann [4] presentan un ejemplo que reproducimos aquı con nues-tros propios resultados, a los efectos de clarificar el metodo de la transformacion“unscented”.

Considerse un robot movil que detecta obstaculos en su ambiente usandoun sonar. El sonar brinda informacion en coordenadas polares (distancia ρ yangulo ϕ) de un sistema de referencia solidario a el. Las coordenadas polares

X =[

ρϕ

]deben ser convertidas a coordenadas cartesianas Y =

[xy

]antes de

ser procesadas por el algoritmo de control del robot. El sonar ha sido optimizadopara operar con una razonablemente buena precision en la medida de distancias(σρ = 2 cm de desviacion estandar), a cambio de una mala precision en lamedida angular (σϕ = 15 ◦ de desviacion estandar). Se mostrara que en estaaplicacion, no resulta apropiado asumir linealidad local de la transformacion decoordenadas Y = f (X) tal que:

[xy

]= f

([ρϕ

])=

[ρ cosϕρ sin ϕ

](56)

cuya matriz jacobiana es: Jf

([ρϕ

])=

[cosϕ −ρ sin ϕsin ϕ ρ cosϕ

].

Supongase que la ubicacion real de un punto donde se encuentra un obstaculo

es X0 =[

ρ0

ϕ0

]=

[1π2

], entonces Y0 =

[x0

y0

]= f

([ρ0

ϕ0

])=

[01

]. Para

apreciar los errores que pueden ser introducidos por linealizacion, en la estima-

cion de los dos primeros momentos de la variable vectorial aleatoria Y =[

xy

]

resultante de la transformacion de X =[

ρϕ

]a traves de la transformacion

(56); se estiman estos momentos mediante tres metodos distintos:

Muestreo Monte Carlo. Se genera un conjunto de N = 106 puntos alea-torios {Xi : i = 1, . . . , N} instanciados de una densidad de distribucion

gaussiana con media E [X] = X0 =[

ρ0

ϕ0

]y covarianza Cov (X, X) =

PX =[

σ2ρ 00 σ2

ϕ

]. Luego, se transforman los puntos Xi para generar la

nube de puntos transformados {Yi = f (Xi) : i = 1, . . . , N} y se estimanlos momentos:

E [Y ] ≈ Y (MC) , 1N

N∑

i=1

Xi , (57)

Cov (Y, Y ) ≈ P(MC)Y , 1

N

N∑

i=1

(Yi − Y (MC)

)(Yi − Y (MC)

)T

. (58)

Transformacion “unscented”. Se aplica la UT tal como se explico antes. Segenera un conjunto de puntos sigma (en este caso cinco puntos) segun (45)a (47); se transforman a traves del cambio de coordenadas y se estimanlos momentos E [Y ] ≈ Y (UT ) y Cov (Y, Y ) ≈ P

(UT )Y de acuerdo con (54)

9

ϕ (rad)

ρ (cm)

y (cm)

x (cm)

Figura 1: Transformacion de coordenadas polares a coordenadas cartesianas. Ala izquierda: plano (ρ, ϕ). A la derecha: plano (x, y). En ambos casos los puntosdel muestreo Monte Carlo se senalan con “+” y los puntos sigma con “♣”.

y (55). Para este ejemplo, se eligieron los siguientes valores para los tresparametros de la trasformacion UT: α = 1, κ = 0 y β = 2.

Linealizacion tal cual se aplica en el contexto del EKF:

E [Y ] ≈ Y (LIN) = Y0 = f (X0) , (59)

Cov (Y, Y ) ≈ P (LIN)y = Jf (X0)PX (Jf (X0))

T. (60)

En la figura 1 se reproducen los puntos del muestreo Monte Carlo junto conlos cinco puntos sigma en el plano (ρ, ϕ) (a la izquierda) y los mismos puntostransformados al plano (x, y) (a la derecha). En la figura 2 se representa elplano (x, y) con las estimaciones de Y (esperanza E [Y ]), indicadas por puntosy los contornos “1 σ” 7 que se desprenden de las estimaciones de PY (matriz decovarianza Cov (Y, Y )); para cada uno de los tres metodos.

Si consideramos que las estimaciones Y (MC), PMCY por muestreo Monte

Carlo son muy buenas estimaciones de los momentos desconocidos E [Y ] yCov (Y, Y ), lo cual es razonable por la enorme cantidad de puntos tomadospara el muestreo; entonces podemos concluir que la estimacion de la estadısticade Y por linealizacion es claramente sesgada e inconsistente. Sesgada porqueY (LIN) 6= E [Y ] e inconsistente porque P

(LIN)Y < Cov (Y, Y ) 8 (se subestima la

incertidumbre real), lo cual se manifiesta en que la elipse “1σ” no es suficiente-mente ancha en la direccion radial. En cambio, la transformacion “unscented”genera una estimacion Y (UT ) mucho mas proxima a E [Y ] y ademas consistente(al menos para este caso en que la distribucion de X es gaussiana).

Dadas estas propiedades de estimacion superiores de la transformacion “uns-cented” sobre la linealizacion tradicionalmente empleada en el contexto del filtrode Kalman extendido y considerando la simplicidad con que se implementa (sinrequerir el calculo de la matriz jacobiana de la transformacion); la UT se havuelto una alternativa atractiva a la linealizacion, para aplicaciones de filtradono lineal.

7El contorno “1 σ” es lugar geometrico de los puntosn

y :�Y − Y

�TP−1

Y

�Y − Y

�= 1o

.8A < B cuando A y B son matrices significa que B −A es definida positiva.

10

-0.3 -0.2 -0.1 0.0 0.1 0.2 0.30.91

0.93

0.95

0.97

0.99

1.01

1.03

+

♦

◊

MCLINUT

+MC♦LIN◊UT

y (cm)

x (cm)

Figura 2: Medias (“+”, “¨” y “♦”) y contornos “1 σ” estimados para la va-riable aleatoria [x, y]T resultante de la transformacion de la variable aleatoria[ρ, φ]T a traves del pasaje de coordenadas polares a cartesianas. En la figura seutilizan las siguientes abreviaciones para indicar el metodo empleado para esti-mar las medias y matrices de covarianza: “MC”: muestreo Monte Carlo, “LIN”:linealizacion de la transformacion y “UT”: transformacion unscented.

2.5. Filtro de Kalman “unscented” (UKF)

El filtro de Kalman “unscented” (UKF: Unscented Kalman Filter) se puedeconsiderar el resultado de incorporar la UT al EKF para mejorar las aproxima-ciones que se hacen de los dos primeros momentos de una variable aleatoria queresulta de propagar otra variable aleatoria (supuesta gaussiana) a traves de unatransformacion no lineal.

Al igual que antes en la presentacion del EKF, el modelo se asume de laforma:

xk = F (xk−1, uk−1, vk−1) (61)

yk = G (xk, nk) . (62)

y se supone que:

p (xk−1 | y1:k−1) = N (xk−1|k−1, Pk−1|k−1

)(63)

p (vk−1) = N (0, Q) (64)

p (nk) = N (0, R) . (65)

Como el modelo no es necesariamente lineal, en el instate siguiente k, las densi-dades de distribucion p (xk | y1:k−1) y p (xk | y1:k) no son necesariamente gaus-sianas. No obstante, al igual que en el EKF, se aproximan estas densidades dedistribucion por densidades gaussianas:

p (xk | y1:k−1) ≈ N (xk|k−1, Pk|k−1

)(66)

11

p (xk | y1:k) ≈ N (xk|k, Pk|k

)(67)

donde ahora el calculo de xk|k−1, Pk|k−1, xk|k y Pk|k implica propagar puntossigma a traves del modelo como se explica a continuacion.

Resulta conveniente definir una nueva variable aleatoria que se llama “vectorde estado aumentado” de dimension L, que incluye el ruido de proceso y el ruidode medida:

xak−1 =

xk−1

vk−1

nk

(68)

de esperanza:

xak−1 =

xk−1|k−1

00

, (69)

y covarianza:

P ak−1 =

Pk−1|k−1 0 00 Q 00 0 R

. (70)

Para esta nueva variable aleatoria se genera un conjunto de puntos sigma

X ai,k−1 =

X x

i,k−1

X vi,k−1

Xni,k

i = 1, . . . , 2L + 1 (71)

tal que:X a

0,k−1 = xak−1 (72)

X ai,k−1 = xa

k−1 +(√

(L + λ) P ak−1

)i

i = 1, . . . , L (73)

X ai,k−1 = xa

k−1 −(√

(L + λ)P ak−1

)i−L

i = L + 1, . . . , 2L ; (74)

donde λ se define igual que en 2.4. Propagando estos puntos sigma (o masprecisamente algunas de sus componentes) a traves de F y G y utilizando lassumas ponderadas (54) y (55), se calculan:

xk|k−1 =2L∑

i=0

W(m)i X x

i,k|k−1 ≈ E [xk | y1:k−1] (75)

Pk|k−1 =2L∑

i=0

W(c)i

(X x

i,k|k−1 − xk|k−1

)(X x

i,k|k−1 − xk|k−1

)T

≈

≈ Cov (xk, xk | y1:k−1) (76)

dondeX x

i,k|k−1 = F(X x

i,k−1, uk−1,X vi,k−1

)(77)

yxk|k = xk|k−1 + Pxkyk

P−1ykyk

(yk − yk|k−1

) ≈ E [xk | y1:k] (78)

Pk|k = Pk|k−1 − PxkykP−1

ykykPT

xkyk≈ Cov (xk, xk | y1:k) (79)

12

donde

yk|k−1 =2L∑

i=0

W(m)i Yi,k|k−1 ≈ E [yk | y1:k−1] (80)

Yi,k|k−1 = G(X x

i,k|k−1,Xni,k

)(81)

Pxkyk=

2L∑

i=0

W(c)i

(X x

i,k|k−1 − xk|k−1

) (Yi,k|k−1 − yk|k−1

)T ≈

≈ Cov (xk, yk | y1:k−1) (82)

Pykyk=

2L∑

i=0

W(c)i

(Yi,k|k−1 − yk|k−1

) (Yi,k|k−1 − yk|k−1

)T ≈

≈ Cov (yk, yk | y1:k−1) . (83)

Al igual que el EKF, como el UKF es recursivo, en realidad se aproximala distribucion de probabilidad real del estado xk, en cada instante k, por unadistribucion gaussiana.

Como el UKF no requiere del calculo de matrices jacobianas, presenta unacomplejidad de calculo numerico comparable a la del EKF 9 y ademas generaestimaciones de E [xk | y1:k] de mayor orden que el EKF; se ha vuelto una herra-mienta corriente para abordar el problema de estimacion de estado (en tiemporeal) de un sistema dinamico no-lineal.

3. Aplicacion

En esta seccion ejemplificamos la aplicacion de las dos extensiones del filtrode Kalman antes presentadas, en el contexto de la teorıa de control no-lineal.Se toma como caso de estudio un problema de control clasico que se ha vueltoun problema tipo sobre el cual ensayar y comparar nuevos metodos de control.Este problema que se describe mas adelante y se conoce como “control de unpendulo invertido”, tiene sus orıgenes historicos en el problema de control de lapropulsion de un cohete.

3.1. La planta a controlar

La planta a controlar se representa, en forma simplificada, en la figura 3.Consiste en un pendulo invertido de masa m montado sobre un carro de masa Mque desliza sobre un riel horizontal y sobre el que un motor (no representado enla figura) aplica una fuerza F mediante una cinta de transmision tensa (tampocorepresentada en la figura). El pendulo tiene un momento de inercia I respectoa un eje perpendicular al plano de la figura que pasa por su centro de masa. El

9El UKF requiere O(L3) operaciones por paso, siendo L la dimension del vector de estadoaumentado; mientras que el EKF requiere O(N3) operaciones por paso, siendo N la dimensiondel vector de estado. Cuando los ruidos de proceso y medida son aditivos, es posible posibleformular el UKF de forma tal que requiera solo O(N3) operaciones por paso. En este casoparticular pero corriente, los filtros presentan el mismo orden de operaciones necesarias porpaso.

13

z

θm, I, L

M

bg

F fr (z)

Figura 3: Representacion de la planta fısica.

centro de masa del pendulo se encuentra a una distancia L de la articulacioncilındrica que lo vincula con el carro. El pendulo y el carro solo se pueden moveren el plano de la figura. Con las variables θ y z se describen respectivamente:el apartamiento angular del pendulo con respecto a la direccion vertical y elapartamiento lineal horizontal del carro con respecto un punto de referenciafijo. La aceleracion gravitatoria se supone constante y se denota g.

Se asume que la fuerza de rozamiento horizontal que actua sobre el carrosolo depende de la velocidad z del carro y que es de la forma

fr (z) =

(µk + (µs − µk) exp

(−

( |z|vs

)δ))

tanh (σz) + νz , (84)

donde µk, µs, vs, δ, ν y σ son constantes. Este modelo de friccion, que combinarozamiento estatico, rozamiento dinamico y rozamiento viscoso, es una adapta-cion de un modelo propuesto por Bo y Pavelescu citado en [2]. La adaptacionconsiste en utilizar la funcion tanh(σ·) (con σ À 1), en vez de la funcion sign(·)de la propuesta original. Mediante esta regularizacion de la funcion signo se re-mueve la discontinuidad en z = 0. Esto permite estar en condiciones de aplicartecnicas de integracion numerica de ecuaciones diferenciales ordinarias sencillas,como el metodo Runge-Kutta que se utiliza mas adelante. En la figura 4 se re-presenta graficamente la funcion (84) para el juego de parametros elegido paradefinir el rozamiento que actua horizontalmente sobre el carro. Estos parame-tros junto a los restantes parametros de la planta se resumen mas adelante enel cuadro 2.

Para el rozamiento que actua en la articulacion cilındrica, se asume un mo-delo viscoso sencillo. Mas precisamente, el torque por rozamiento que, en laarticulacion cilındrica, se ejerce sobre el pendulo, se asume que es de la formabθ con b constante.

3.1.1. Dinamica de la planta en tiempo continuo

Bajo las hipotesis recien expuestas, la dinamica de la planta se puede ex-presar matematicamente mediante una representacion en variables de estado,

14

-0.5 -0.4 -0.3 -0.2 -0.1 0.0 0.1 0.2 0.3 0.4 0.5-1.0

-0.8

-0.6

-0.4

-0.2

0.0

0.2

0.4

0.6

0.8

1.0

fr (N)

z (m/s)

Figura 4: Modelo de friccion.

no-lineal y en tiempo continuo:

dx

dt= fplanta (x, u) =

=

x2

gL (1+ M

m ) sin x1−x22 sin x1 cos x1− g

L1

mg (u−fr(x4)) cos x1− 1+ Mm

mL2 bx2

(1+ Mm )(1+ I

mL2 )−cos2 x1

x4

L− g

LmL2

I+mL2 sin x1 cos x1+x22 sin x1+

gL

1mg (u−fr(x4))+

1I+mL2 bx2 cos x1

Mm +1− mL2

I+mL2 cos2 x1

(85)

donde x1 , θ, x2 , θ, x3 , z, x4 , z son las componentes del vector de estadox , [x1 x2 x3 x4]

T y donde u , F es la entrada.

3.1.2. El objetivo de control

El objetivo de control consiste en lograr que (asintoticamente) el pendulo semantenga en su posicion “invertida” x1 ≈ 0, sin apartar demasiado el carro deuna posicion de referencia fija x3 ≈ 0.

Se supone que en los instantes 0, T, 2T, . . . , kT, . . . con k ∈ N, se puedentomar medidas ruidosas:

yk =[

y1,k

y2,k

]= G (xk, nk) =

[x1,k

x3,k

]+ nk (86)

donde xk = [x1,k , x2,k , x3,k , x4,k]T = [x1(kT ) , x2(kT ) , x3(kT ) , x4(kT )]T ,yk = [y1,k y2,k]T = [x1(kT ) , x3(kT )]T y donde se asume que el proceso es-tocastico {nk}k∈N es ruido blanco gaussiano de media nula:

Cov (nk, nj) = δ (k − j) R ∀k, j ≥ 0 (87)

p (nk) = N (0, R) (88)

15

Parametro Valor Unidadσθ 18 ◦

σz 2 mm

Cuadro 1: Desviaciones estandar de las medidas.

y matriz de covarianza:

R =[

σ2θ 00 σ2

z

]; (89)

siendo σθ y σz las desviaciones estandar que caracterizan la precision de los ins-trumentos que se utilizan para medir las variables de estado accesibles: x1 = θy x3 = z en los instantes de muestreo. En el cuadro 1 se indican los valorestomados para estos parametros. Se intento emular una situacion practica enla que se dispone de un instrumento relativamente bueno para medir la posi-cion del carro, mientras que el instrumento con el que se cuenta para medir elapartamiento del pendulo respecto a la vertical, es bastante impreciso.

A partir de las medidas (86) el controlador a disenar en tiempo discreto,debe sintetizar una senal {uk}k∈N que se convierte a tiempo continuo me-diante un mantenedor de orden cero (MOC), que impone u(t) = uk parat ∈ [kT, (k + 1) T ) a la planta de tiempo continuo. La senal {uk}k∈N que au-tomaticamente sintetiza el controlador digital, debe ser tal que para k → +∞se alcance el objetivo de control.

3.1.3. Dinamica de la planta en tiempo discreto

A los efectos de simular la dinamica de la planta en una computadora digital,se discretiza (85) utilizando el metodo de integracion numerica de Runge-Kuttade cuarto orden y paso de integracion fijo h.

Resultara conveniente elegir el paso de integracion h igual a un submultiplodel perıodo de muestreo T , es decir T = Hh con H ∈ N+ 10.

El tiempo queda entonces discretizado en los instantes t = 0, h, 2h, . . . , jh, . . .con j ∈ N y la entrada u (t) (que es impuesta por el MOC, cuyo perıodo de mues-treo es T ) se mantiene constante e igual a uk en los intervalos [kT, (k + 1) T ) =[kT, kT + h) ∪ . . . [kT + (H − 1)h, kT + Hh) para k ∈ N.

Esta discretizacion da origen a un sistema dinamico en tiempo discreto des-crito por:

xk = Fplanta (xk−1, uk−1) = FRK ◦ · · · ◦ FRK︸ ︷︷ ︸H

(xk−1, uk−1) =

= FHRK (xk−1, uk−1) ; (90)

donde:

FRK (x, u) = x +k1 (x, u)

6+

k2 (x, u)3

+k3 (x, u)

3+

k4 (x, u)6

; (91)

10N+ = N \ {0} es el conjunto de los numeros naturales positivos.

16

Parametro Valor Unidadµs 1,3 Nµk 0,2 Nν 0, 3 Ns

mvs 0, 005 m

sδ 1m 0, 09 kgM 2, 2 kgI 0, 003 kg m2

L 0, 45 mg 9, 8 m

s2

b 0, 001 N m sh 0, 0025 sH 4

Cuadro 2: Parametros.

siendo:k1 (x, u) = hfplanta (x, u) ,

k2 (x, u) = hfplanta

(x + k1(x,u)

2 , u)

,

k3 (x, u) = hfplanta

(x + k2(x,u)

2 , u)

,

k4 (x, u) = hfplanta (x + k3 (x, u) , u) .

(92)

El sistema dinamico de tiempo discreto (90) aproxima (con H pasos de inte-gracion numerica) la transicion del vector de estado x(t) del sistema dinamico detiempo continuo (85), cuando el tiempo avanza de t = (k − 1)T a t = (kT )− su-poniendo que en este intervalo la entrada u(t) permanece constante igual a uk−1

debido al MOC. Este metodo aproximado para simular un sistema dinamico detiempo continuo es aceptable si el paso de integracion h se elige adecuadamente(suficientemente chico como para que el error de truncamiento sea despreciabley suficientemente grande como para que los errores de redondeo tambien seandespreciables).

En este trabajo, se supone que (90) describe exactamente la dinamica de laplanta a controlar. Esto es a los efectos de poder “experimentar” numericamentecon una planta, sin necesidad de construir un prototipo real. En el cuadro 2se presentan los valores elegidos para cada uno de los parametros del sistemadinamico de tiempo discreto, que de ahora en mas, se considerara equivalente ala planta real.

Los valores del cuadro 2 se eligieron arbitrariamente dentro de los rangosfısicamente plausibles para los parametros; pero cuidando que el paso de inte-gracion h fuese ordenes de magnitud menor que la escala temporal en la quese aprecian variaciones significativas del vector de estado, cuando se simula larelajacion de la planta desde la condicion inicial xk =

[π4 , 0 , 0 , 0

]T .

3.2. Modelado

En esta sub-seccion se introduce un modelo de la planta descrita anterior-mente. Como en cualquier proceso de diseno de un controlador automatico ba-

17

z

θm

M

gL

F

Figura 5: Representacion esquematica del modelo.

sado en un modelo, la primer etapa consiste en el modelado de la planta, enla que se hacen ciertas simplificaciones de la realidad, en un compromiso entrerealismo y tratabilidad matematica.

En la figura 5, se reproduce un esquema del modelo. Se supone que la masam del pendulo esta concentrada en su centro de masa a una distancia L de laarticulacion cilındrica que lo vincula con el carro de masa M , sobre el que seejerce una fuerza horizontal F . Se desprecia entonces, el efecto de la inercia nonula del pendulo real. Se desprecian ademas todas las fricciones, tanto en elmovimiento horizontal del carro, como en el pivoteo del pendulo.

Bajo estas hipotesis, el modelo dinamico de la planta se puede formularcomo:

dxdt

=ddt

x1

x2

x3

x4

= f (x, u) =

=

x2gL (1+ M

m ) sin x1−x22 sin x1 cos x1− g

L1

mg u cos x1Mm +sin2 x1

x4

L− g

L sin x1 cos x1+x22 sin x1+

gL

1mg u

Mm +sin2 x1

(93)

donde x1 = θ, x2 = θ, x3 = z, x4 = z y u = F .Este modelo de tiempo continuo es discretizado mediante el metodo de in-

tegracion numerica de Runge-Kutta de cuarto orden, en forma completamenteanaloga a como se procedio con el modelo de tiempo continuo de la planta ycon el mismo paso de integracion h. Es decir en, (90), (91) y (92) se sustituyefplanta por f y se aproxima entonces (93) por el modelo de tiempo discreto:

xk = F ′ (xk−1, uk−1) = FRK ◦ · · · ◦ FRK︸ ︷︷ ︸H

(xk−1, uk−1) =

= FHRK (xk−1, uk−1) . (94)

Los parametros del modelo se estiman en los valores que se resumen en elcuadro 3.

18

Parametro Valor Unidadm 0, 1 kgM 2 kgL 0, 5 mg 9, 8 m

s2

h 0, 0025 sH 4

Cuadro 3: Parametros del modelo.

Observese que los valores del cuadro 3 difieren un 10 % de los valores co-rrespondientes en el cuadro cuadro 2 donde se resumen los parametros de laplanta “real”; excepto por el valor de la aceleracion gravitatoria g, el paso deintegracion numerica h y la cantidad de pasos de integracion por perıodo demuestreo H. Esto pretende reproducir la situacion practica real, en la que losparametros se determinan solo aproximadamente.

Las diferencias existentes entre la planta (90) y el modelo (94), originadasen las simplificaciones introducidas en el modelo (momento de inercia y fric-ciones despreciadas, ası como diferencias en los parametros: m, M y L); hacenque el comportamiento dinamico de estos dos sistemas de tiempo discreto, seaobviamente distinto. A modo de ejemplo, en la figura 6 se reproduce la evo-lucion temporal de las cuatro componentes del vector de estado del modelo yde la planta, cuando no se hace ninguna fuerza sobre el carro y se parte de lacondicion inicial

[π4 , 0 , 0 , 0

]T . Observese el efecto del rozamiento en la articu-lacion cilındrica atenuando la amplitud de las oscilaciones de x1 y el efecto delrozamiento estatico al que esta sometido el carro, que llega a detenerlo comple-tamente (x4 = 0) durante algunos intervalos de tiempo.

Considerando que el modelo (94) es una descripcion inexacta del compor-tamiento dinamico de la planta “real”, se agrega al mismo una componenteestocastica o ruido de proceso, con la que se pretende compensar las carenciasdel modelado. Se asume que este ruido es aditivo. De esta forma, el modelodefinitivo es:

xk = F (xk−1, uk−1) + vk ; (95)

donde se asume que el proceso estocastico {vk}k∈N es ruido blanco gaussiano demedia nula:

Cov (vk, vj) = δ (k − j)Q ∀k, j ≥ 0 (96)

p (vk) = N (0, Q) . (97)

La matriz Q que caracteriza al ruido de proceso, se especifica mas adelante yresultara de la sintonıa manual del EKF utilizado como observador de estadode la planta.

3.3. Algoritmo de control

Para alcanzar el objetivo de control, se utiliza el metodo de control no-linealconocido como metodo de la ecuacion de Riccati dependiente del estado paratiempo discreto (discrete-time SDRE: State Dependent Riccati Equation) [6].

19

0 2 4 6 8 10 12 14 16 18 200123456

0 2 4 6 8 10 12 14 16 18 20-9-7-5-3-113579

0 2 4 6 8 10 12 14 16 18 20-0.010.010.030.050.070.090.110.130.150.17

0 2 4 6 8 10 12 14 16 18 20-0.20-0.16-0.12-0.08-0.040.000.040.080.120.160.20x4 (m

s )

x3 (m)

x2 ( rads )

x1 (rad)

t (s)

t (s)

t (s)

t (s)

Figura 6: Trayectorias generadas por el modelo (trazo discontinuo) y la planta(trazo continuo), partiendo de la misma condicion inicial [x1,0, x2,0, x3,0, x4,0]

T =[π4 , 0, 0, 0

]T . El intervalo de tiempo continuo representado en la figura[0 , 20, 01 s) corresponde a la simulacion de los instantes de tiempo discreto:k=0, 1, . . . , 2000; siendo T = Hh = 4 × 0, 0025 s = 0, 01 s el perıodo de mues-treo.

20

El primer paso de esta metodologıa de control consiste en expresar la dinami-ca del modelo de tiempo continuo en la forma pseudo-lineal:

dxdt

= f (x, u) = A(x)x + B(x)u , (98)

en la que los coeficientes dependen del estado x.En el caso del modelo (93) que nos ocupa, tomamos A(x) y B(x) de la

siguiente forma:

A (x) =

0 1 0 0gL (1+ M

m )−x22 cos x1

Mm +sin2 x1

sinc x1 0 0 00 0 0 1

L− g

L cos x1+x22

Mm +sin2 x1

sinc x1 0 0 0

, (99)

B (x) =

0

−gL

1mg cos x1

Mm +sin2 x1

0

LgL

1mg

Mm +sin2 x1

; (100)

donde sinc x1 ,{

sin x1x1

si x1 6= 01 si x1 = 0

.

Seguidamente, se discretiza (98) como si se tratara de un modelo lineal x =Ax + Bu en el que A y B fueran constantes. Es decir:

xk = Ad (xk−1)xk−1 + Bd (xk−1) uk−1 ; (101)

dondeAd (xk−1) = exp (A (xk−1))T , (102)

Bd (xk−1) =

(∫ T

0

exp (A (xk−1) τ) dτ

)B (xk−1) . (103)

En esta discretizacion, se asume implıcitamente que el perıodo de muestreo Tes suficientemente pequeno, como para que el vector de estado x(t) no varıesignificativamente en un intervalo de tiempo de duracion T .

Una vez obtenido el modelo en tiempo discreto (101), el problema de con-trol se plantea en forma analoga al problema de control optimo cuadratico dehorizonte infinito para sistemas lineales. Mas precisamente, se introduce unafuncion de costo a minimizar:

J =12

+∞∑

k=0

(xT

k Q (xk)xk + uTk R (xk)uk

)(104)

donde las matrices Q (xk) y R (xk) ponderan la incidencia en el costo del apar-tamiento del vector de estado xk respecto al origen y el apartamiento de laaccion de control uk respecto al origen, respectivamente. De esta forma, en ladefinicion de estas matrices se expresa la eleccion del disenador en lo que refiereal compromiso entre la performance del regulador y la magnitud de la accion de

21

Parametro Valor

Q

10 0 0 00 1 0 00 0 100 00 0 0 1

R [10]

Cuadro 4: Matrices que definen el costo (104).

control necesaria para lograrlo. Se requiere que Q (x) sea semi-definida positivay R (x) definida positiva para todo x. La solucion a este problema es:

uk = −K (xk) xk (105)

conK (xk) =

(R (xk) + BT

d P (xk)Bd

)−1BT

d P (xk)Ad , (106)

siendo P (xk) la solucion de la ecuacion de Riccati en estado estacionario:

P (xk) = Q (xk) + ATd P (xk)Ad −

−ATd P (xk)Bd

(R + BT

d P (xk)Bd

)−1BT

d P (xk) Ad . (107)

Tanto en (106) como en (107) se ha omitido la dependencia explıcita de Ad yBd con respecto a xk, para simplificar la notacion.

Para el control del pendulo invertido, las matrices Q y R se tomaron como seindica en el cuadro 4. Claramente, el enfasis en la definicion del costo, esta puestoen la regulacion de las variables de posicion x1 y x3.

3.4. Observador de estado

La solucion (105) al problema de minimizar (104) es una realimentacion deestado con la particularidad que la ganancia (106) depende del estado xk. Paraque este metodo de control por realimentacion de estado sea implementable, esnecesario que todas las componentes del vector de estado sean accesibles. Talcomo se formulo el problema de control del pendulo invertido en 3.1.2; de lascuatro variables de estado x1 = θ, x2 = θ, x3 = z y x4 = z, solo se pueden medir(con cierto error de medida) las variables de posicion y1 = x1 = θ y y2 = x3 = z.Entonces, para poder aplicar el metodo de control recien descrito al problemadel control del pendulo invertido, es necesario disenar un observador de estadocompleto que brinde la “mejor” estimacion posible del vector de estado completo,a partir de las medidas ruidosas que se van adquiriendo en tiempo real y unmodelo imperfecto de la planta a controlar. Esta, es justamente la funcionalidadde las dos extensiones del filtro de Kalman para sistemas no lineales presentadasen 2.2 y 2.5.

El diagrama de bloques de la figura 7 representa el sistema realimentadocompleto. La planta es comandada por la entrada uk y es regida por la dinamicade tiempo discreto (90). De la misma se obtienen medidas ruidosas (86) que junto

22

CONTROLADOROBSERVADOR

PLANTAuk yk

xk|k

Figura 7: Diagrama de bloques del sistema realimentado.

con uk van al observador de estado. El observador combina estas dos senalesjunto con el conocimiento que tiene de la planta (el modelo 95, la estadısticadel ruido de proceso vk y la estadıstica del ruido de medida nk) para generarestimaciones xk|k del vector de estado de la planta xk. Con estas estimaciones sesintetiza la accion de control uk con (105), como si se tratara del verdadero vectorde estado xk. En el contexto de la teorıa de control optimo lineal, esta formade proceder esta sustentada en el el principio de separacion, que asegura que laestrategia de control optima (en el sentido de minimizar 104) es justamente laque resulta de disenar, por un lado el estimador optimo y por otro, el reguladoroptimo; en forma independiente. Cuando se procede de la misma forma en elcaso de un sistema no-lineal, el principio de separacion ya no es valido y por lotanto la metodologıa de control resultante no es necesariamente optima, aunquees una solucion factible.

Notese que si bien la dinamica del modelo (95) que maneja el observador,no coincide con la dinamica de la planta (90); en lo que refiere a las medidas, elobservador maneja el mismo modelo de medida G (xk, nk) con el que se generanlas medidas (86) de la planta. Es decir, se asume perfecto conocimiento delproceso de medida. Esto tiene sentido porque en el caso que nos ocupa, G esmuy sencilla; simplemente extrae la primera y tercera componente del vector deestado y adiciona el ruido de medida caracterıstico de los instrumentos.

3.5. Resultados de las simulaciones

A continuacion se exponen los resultados de las simulaciones del sistemarepresentado en la figura 7 operando: 1) en lazo abierto (sin la realimentacionrepresentada en la figura desde la salida del controlador hasta la entrada de laplanta) y 2) en lazo cerrado (tal cual se encuentra representado en la figura). Encada caso, se utiliza el EKF y el UKF como observador de estado y se comparanlos desempenos.

En todos los casos se genero ruido de medida estocastico de acuerdo conla misma estadıstica suministrada a los filtros, es decir se simulo el ruido demedida generando muestras aleatorias de:

nk ∼ N (0, R) 11. (108)

El ruido no se simulo explıcitamente, porque esta implıcito en las diferencias yamencionadas entre el modelo y la planta.

11x ∼ N (x, Px) significa que x es una variable aleatoria de esperanza x y covarianza Px.

23

Todo el trabajo de programacion que implico realizar las simulaciones yprocesar los resultados, se realizo utilizando el software libre Scilab [9].

3.5.1. Sintonizacion en lazo abierto

Hasta ahora no se ha especificado como se eligio la matriz de covarianzaQ que describe al ruido de proceso de media nula. Esta matriz y la matrizde covarianza R que describe al ruido de medida de media nula, deben sersuministradas al filtro que se utiliza como observador de estado (ya sea el EKF oel UKF). La eleccion de estas matrices influye drasticamente en la performancedel filtro para estimar el estado de la planta y su eleccion es referida comosintonizacion del filtro.

La eleccion de la matriz R (ver cuadro 1 y expresion (89)), se hizo conside-rando la precision de los supuestos instrumentos de medida. La matriz Q es masdifıcil de determinar, porque en general no se conoce la exactitud con la que elmodelo suministrado al filtro, es capaz de representar la dinamica de la planta“real”.

La matriz Q se eligio por tanteo, simulando el sistema planta-observadoren lazo abierto (sin realimentacion) y con entrada {uk}k∈N nula, partiendo del

estado inicial x0 =[

π4 , 0 , 0 , 0

]T . Luego de unas cuantas simulaciones, proban-do con diferentes valores para Q y evaluando el error cometido por el EKF enla estimacion, instante a instante, de las variables de estado; se decidio elegirla matriz Q como se indica en el cuadro 5. Esta forma de sintonizar el filtropuede ser criticada desde un enfoque practico, ya que en la realidad solo secuenta con medidas ruidosas de algunas variables de estado. No obstante, no sebusco sintonizar el filtro hasta su desempeno optimo, sino simplemente definiruna matriz Q que generara estimaciones aceptables y sobre todo, que el filtrotuviera la capacidad dinamica de seguir razonablemente bien, los cambios en elvector de estado.

Como se puede apreciar en el cuadro 5, se dio mayor peso a los elementosde la diagonal y entre estos a aquellos de las filas 2 y 4 que corresponderıan ala discretizacion de las ecuaciones mecanicas cardinales del modelo de tiempocontinuo (93), en las que podrıan faltar fuerzas o momentos no modelados.Los restantes elementos se fijaron arbitrariamente en el valor 1 × 10−6. Estaforma de proceder, para variar solo aquellos elementos de Q que se suponeson mas significativos, es heurıstica y nada rigurosa; aunque resulto util en lapractica para tantear con solo cuatro parametros, en vez de los diez parametrosnecesarios para determinar la matriz Q que debe ser simetrica. En rigor, enla discretizacion (94) del modelo de tiempo continuo, surge una nueva matrizde transicion de estado, con la que se pierde la correspondencia anteriormentereferida entre las filas 2 y 4 y las ecuaciones mecanicas cardinales. No obstante,aun ası, es esperable que sea en estas filas de la nueva matriz de transicion deestado, donde tengan mayor efecto las carencias del modelado.

En la figura 8 se reproduce (en trazo negro) la evolucion temporal de cadavariable de estado de la planta partiendo del estado inicial x0 =

[π4 , 0 , 0 , 0

]T

y las trayectorias correspondientes estimadas por el EKF (en azul) y el UKF(en rojo); para la eleccion realizada de las matrices Q y R. Se observa que eldesempeno de ambos filtros es similar, salvo para la cuarta componente delvector de estado, para la cual el UKF brinda mejores estimaciones que el EKF.

24

Parametro Valor

Q

1× 10−4 1× 10−6 1× 10−6 1× 10−6

1× 10−6 1 1× 10−6 1× 10−6

1× 10−6 1× 10−6 1× 10−4 1× 10−6

1× 10−6 1× 10−6 1× 10−6 5× 10−2

Cuadro 5: Matriz de covarianza del ruido de proceso.

0 100 200 300 400 5000123456

0 100 200 300 400 500-11

-7-3159

13

0 100 200 300 400 500-0.02-0.010.000.010.020.030.04

0 100 200 300 400 500-0.4-0.3-0.2-0.10.00.10.20.30.4

EKFUKF

EKFUKF

EKFUKF

EKFUKF

x4(ms )

x3(m)

x2( rads )

x1(rad)

k

k

k

k

Figura 8: Estimacion de las cuatro componentes del vector de estado de la plantaoperando en lazo abierto y con entrada nula. En negro: evolucion temporal delas variables de estado de la planta; en azul: estimaciones generadas por el EKFy en rojo: estimaciones generadas por el UKF.

25

En la figura 9, se muestra el andamiento temporal del error cuadratico mediopara cada uno de los dos filtros; el cual se estima promediando entre cien expe-riencias (de quinientos instantes de muestreo cada una) en lazo abierto y entradanula, con los dos filtros estimando el estado de la planta simultaneamente. Encada una de estas experiencias, el estado inicial de la planta se eligio aleatoria-mente segun:

x0 ∼ N

0000

,

π2 0 0 00 19 0 00 0 1 00 0 0 10

(109)

y las condiciones iniciales para los filtros se tomaron siempre iguales a:

x0|0 =

0000

P0|0 =

π2 0 0 00 19 0 00 0 1 00 0 0 10

. (110)

Observese que el error cuadratico medio, ası estimado, tiene un andamiento muysimilar para los dos filtros, apenas un poco mejor para el EKF que para el UKF.La mejor performance del UKF en la estimacion de la cuarta componente delvector de estado, senalada anteriormente para la experiencia particular cuyosresultados se reproducen en la figura 8; pudo verificarse en todas las experiencias,pero su incidencia en el error cuadratico es despreciable frente al los errores quese cometen en la estimacion de las dos primeras componentes del vector deestado. Por esto, no se refleja en las curvas del la figura 9.

3.5.2. Estimacion de estado en lazo cerrado

A continuacion se presentan los resultados de las simulaciones del sistemacompleto, tal cual se representa en la figura 7, es decir con la realimentacionoperando para controlar planta. Para estas simulaciones, se mantuvieron lasmismas matrices Q y R elegidas como ya se explico anteriormente.

Al igual que antes para la operacion en lazo abierto, se realizaron cien expe-riencias (ahora de mil instantes de muestreo cada una), en las que el EKF y elUKF estimaban el estado de la planta. A diferencia de antes, en que los filtrosestimaban el estado de una “misma” planta bajo observacion, en este caso sedebieron simular dos plantas “independientes” ya que cada una se comandabacon la accion de control generada con las estimaciones provenientes de cada unode los dos filtros. Lo que sı se impuso en forma identica y simultanea para elEKF y el UKF, fue el ruido de medida instantaneo generado aleatoriamente, elestado inicial de planta elegido aleatoriamente segun 109 y la condicion inicialpara los filtros siempre segun 110.

Se programo la detencion automatica de cualquier filtro cuyo error cuadraticode estimacion del estado real de la planta, superara una cota maxima de valor100, dando por imposible el control de la planta una vez alcanzada esta magnituden el error. De las 100 experiencias realizadas con los dos filtros, operandosimultaneamente sobre plantas independientes; en 89 de las mismas el EKFalcanzo esta cota de error, mientras que el UKF la alcanzo solamente en 15.En la figura 10 se ejemplifica uno de estos casos en que el EKF alcanzaba estacota maxima de error cuadratico. Es claro que en casos como este se perdıa el

26

0 100 200 300 400 5000

1

2

3

4

5

6EKFUKF

Promedio del error cuadratico ≈ MSE

k

Figura 9: Estimacion del error cuadratico medio en la estimacion del estado dela planta, operando en lazo abierto y con entrada nula, para el EKF (en azul)y el UKF (en rojo). La estimacion, instante a instante, del error cuadraticomedio se obtiene promediando entre 100 experiencias distintas de 500 instantesde muestreo cada una.

27

0 20 40 60 80 100 120-0.6-0.20.20.61.01.41.82.22.63.03.4

0 20 40 60 80 100 120-80-60-40-20

020406080

100

0 20 40 60 80 100 120-120-100

-80-60-40-20

0

0 20 40 60 80 100 120-500-400-300-200-100

0100

0 20 40 60 80 100 120-13000-11000

-9000-7000-5000-3000-100010003000u(N)

x4(ms )

x3(m)

x2( rads )

x1(rad)

k

k

k

k

k

Figura 10: Evolucion temporal de las variables de estado de la planta y accionde control (todas en trazo continuo), en un caso en el que el EKF no es capaz degenerar estimaciones (en trazo discontinuo) suficientemente buenas como paraconseguir controlar la planta.

control de la planta. Solo en una experiencia se dio que el UKF alcanzaba lacota maxima de error cuadratico mientras que el EKF no (ver figura 11).

En la figura 12, se muestra el andamiento temporal del error cuadraticomedio para cada uno de los dos filtros; el cual se estima promediando entreaquellas experiencias para las cuales el filtro del que se trate, no alcanzo la cotamaxima de 100 en el error cuadratico. Al igual que para las experiencias en lazoabierto, el estado inicial de la planta se eligio aleatoriamente segun 109 y lascondiciones iniciales para los filtros se tomaron siempre segun 110. Observese,que en lazo cerrado, el desempeno del UKF es claramente superior al del EKF.Esto tambien se puede apreciar en las 10 experiencias en las cuales ninguno delos dos filtros alcanzaron la cota maxima aceptada para el error. Los resultadosde una de tales experiencias, se reproducen en la figura 13. A simple vista seobserva que el control de la planta es superior cuando se utiliza el UKF comoobservador, que cuando se utiliza el EKF como observador. Incluso la accion decontrol uk es mas moderada en cuando se utiliza el UKF, que cuando se utilizael EKF.

Otra forma de comparar los desempenos de ambos filtros consistio en evaluarel costo (104) tomando un numero finito de terminos (tantos como instantesde muestreo simulados). Debe aclararse que el costo (104) converge cuando sedisena un control optimo de horizonte infinito para un sistema lineal en uncontexto determinista. Cuando se pretende extender los resultados de la teorıa

28

0 100 200 300 400 500 600 700 800 900 1000-6-5-4-3-2-1012

0 100 200 300 400 500 600 700 800 900 1000-170-130

-90-50-103070

110

0 100 200 300 400 500 600 700 800 900 1000-100100300500700900

11001300

0 100 200 300 400 500 600 700 800 900 1000-100300700

110015001900230027003100

0 100 200 300 400 500 600 700 800 900 1000-20000-10000

010000200003000040000500006000070000

EKFUKF

EKFUKF

EKFUKF

EKFUKF

EKFUKF

u(N)

x4(ms )

x3(m)

x2( rads )

x1(rad)

k

k

k

k

k

Figura 11: Evolucion temporal de las variables de estado de la planta y la accionde control que la comanda. En azul: EKF como observador. En rojo: UKF comoobservador. Esta fue la unica experiencia, de las 100 realizadas, en que el UKFperdio el control de la planta mientras que el EKF no.

29

0 100 200 300 400 500 600 700 800 900 10000

1

2

3

4

5

6

7EKFUKF

Promedio del error cuadratico ≈ MSE

k

Figura 12: Estimacion del error cuadratico medio en la estimacion del estadode la planta, operando en lazo cerrado, para el EKF (en azul) y el UKF (enrojo). La estimacion, instante a instante, del error cuadratico medio se obtienepromediando entre 100 experiencias distintas de 500 instantes de muestreo cadauna.

30

0 100 200 300 400 500 600 700 800 900 1000-0.9-0.7-0.5-0.3-0.10.10.30.50.70.91.1

0 100 200 300 400 500 600 700 800 900 1000-7-5-3-1135

0 100 200 300 400 500 600 700 800 900 1000-3-2-101234

0 100 200 300 400 500 600 700 800 900 1000-8-6-4-202468

10

0 100 200 300 400 500 600 700 800 900 1000-300-200-100

0100200300400500600

EKFUKF

EKFUKF

EKFUKF

EKFUKF

EKFUKF

u(N)

x4(ms )

x3(m)

x2( rads )

x1(rad)

k

k

k

k

k

Figura 13: Evolucion temporal de las cuatro variables de estado de la plantay la accion de control que la comanda, cuando se utiliza el EKF (en azul) oalternativamente el UKF (en rojo) como observador de estado.

31

0 10 20 30 40 50 60 70 80 90 1000e+00

1e+08

2e+08

3e+08

4e+08

5e+08

6e+08

7e+08EKFUKF

Costo

k

Figura 14: Costo (104) truncado a un numero finito de terminos, para cadauna de las cien experiencias, cuando la planta es controlada utilizando el EKFcomo observador (barras en azul) o el UKF como observador (barras en rojosuperpuestas a las azules).

de control optimo lineal, mediante el uso de la SDRE y ademas se aplica en uncontexto estocastico como el que estamos tratando; la serie (104) no tiene porque converger. No obstante, la suma truncada a un numero finito de terminospuede ser utilizada para comparar desempenos.

En la figura 14 se representa para cada una de las cien experiencias, el costoasociado al EKF por una barra azul y el costo asociado al UKF por una barraroja superpuesta. La ausencia de una barra significa que el filtro alcanzo la cotamaxima de 100 en el error cuadratico de estimacion y por lo tanto la experienciafue descartada del analisis. Se observa que en las 10 experiencias, en que tantoel error del EKF como el del UKF se mantuvo por debajo de la cota fijada; elcosto asociado al UKF fue siempre menor.

4. Conclusiones

En este trabajo se expusieron dos extensiones del filtro de Kalman parasistemas no lineales: el EKF (Extended Kalman Filter) y el UKF (UnscentedKalman Filter); desde un marco teorico comun. Mediante un ejemplo sencillode transformacion de coordenadas, se ilustro la aplicacion de la UT (UnscentedTransform), que es la herramienta fundamental subyacente en la formulaciondel UKF.

32

0 10 20 30 40 50 60 70 80 90 1000e+00

1e+07

2e+07

3e+07

4e+07

5e+07

6e+07

7e+07

8e+07

9e+07

1e+08EKFUKF

Costo

k

Figura 15: Ampliacion de la figura 14.

Posteriormente, se aplicaron las dos extensiones del filtro de Kalman comoobservadores de estado completo, al problema de control de un pendulo inverti-do. Se experimento con este problema en forma numerica, pero formulandolo dela forma mas realista posible; distinguiendo entre la planta “real” y un modelo(mas sencillo) empleado para disenar el observador y el controlador.

En lazo abierto y con entrada nula a la planta, se sintonizo manualmente elEKF hasta alcanzar un desempeno aceptable, aunque no necesariamente optimo.Para esta sintonizacion, que se mantuvo incambiada y que se aplico en formaidentica al UKF; se encontro que el desempeno en lazo abierto de los dos filtrosera muy similar.

En lazo cerrado, manteniendo la sintonizacion antes elegida en lazo abiertoy utilizando la misma metodologıa SDRE (State Dependent Riccati Equation)de control no-lineal; se compararon los desempenos del EKF y el UKF operandocomo observadores. En la mayorıa de las experiencias realizadas, se encontro queel control de la planta era superior al utilizar el UKF, que cuando se utilizabael EKF. Esto se pudo apreciar directamente de las respuestas en lazo cerradoobtenidas y tambien mediante la evaluacion de un costo que pondera la re-gulacion del vector estado y la accion de control requerida. Esta superioridades coherente con el hecho de que, en todo instante, el UKF cometıa errorescuadraticos medios menores a los que cometıa el EKF. Como la metodologıa decontrol utilizada se basa en estas estimaciones, resulta razonable que el mejorcontrol se consiga con el observador que comete menor error de estimacion. Masdestacable aun, es que el EKF perdio el control de la planta en el 89 % de lasexperiencias realizadas, mientras que el UKF solo lo perdio en el 15% de las

33

experiencias realizadas.Si bien la forma “artesanal” y poco practica de sintonizar la matriz de co-

varianza Q, que “informa” a los filtros las caracterısticas del ruido de medida,es claramente criticable; consideramos que las experiencias numericas realizadasson suficientes para justificar la viabilidad de estas tecnicas de observacion enproblemas de control no-lineal e ilustrar (al menos para un problema y solucionparticular), la superioridad del UKF sobre el EKF, demostrable en forma rigu-rosa por expansiones en series de potencias de los momentos estimados [8]. Cabedestacar ademas, la facilidad con que se implementa el UKF en comparacioncon el EKF; para el cual fue necesario programar las matrices jacobianas (41)a (44), correspondientes al modelo (95), aplicando la regla de la cadena a lasfunciones compuestas que resultan de los H pasos de integracion entre instantesde muestreo (ver (94)) y del propio metodo de Runge-Kutta (91) y (92).

Otro aspecto criticable de la sintonizacion, es que se realizo en lazo abierto ycon entrada nula. Seguramente, hubiese sido posible alcanzar mejor desempenosintonizando directamente en lazo cerrado, que era el modo de operacion parael cual se estaba disenando el observador. De todas formas, el mayor problema(ya sea en lazo abierto o en lazo cerrado), consiste en encontrar alguna formaracional de determinar la sintonizacion del filtro, sin incurrir en metodos deprueba y error. Mejor aun serıa lograr que el filtro “aprenda” automaticamentelos parametros que caracterizan al ruido de proceso y de medida. Esto ultimopodrıa ser motivo de otro trabajo y no esta demasiado estudiado en la literaturasegun reportan Abbeel et al. en [1]; donde proponen algunos algoritmos desintonizacion automatica que no ensayamos en nuestras experiencias por faltatiempo.

Referencias

[1] Abbeel P., Coates A., Montemerlo M., Ng A. Y. & Thrun S. DiscriminativeTraining of Kalman Filters. Robotics, Science and Systems. Boston, USA,June 2005.

[2] De Marchi J. A. Modeling of dynamic friction, impact backlashand elastic compliance nonlinearities in machine tools, with applica-tions to asymmetric viscous and kninetic friction identification. Tesisde doctorado. Rensselær Polytechnic Institute, Troy, New York, 1988.http://www.perihelia.com/doc/phdweb.pdf .

[3] Julier S. J. & Uhlmann J. K. A new approach for filtering nonlinear systems.Proceedings of tha American Control Conference, 1628–1632. 1995.

[4] Julier S. J. & Uhlmann J. K. A new extension of the Kalman filter to nonli-near systems. Int. Symp. Aerospace/Defense Sensing, Smul. and Controls,Orlando, FL, 1997. http://citeseer.ist.psu.edu/julier97new.html .

[5] Lewis F. L. Optimal estimation. New York: Wiley, 1986.

[6] Menon P. K., Sweriduk G. D., Vaddi S. S. & Ohlme-yer E. J. Nonlinear Discrete-time Design Methods for Mis-sile Flight Control Systems. 2004 AIAA Guidance, Naviga-tion, and Control Conference Providence, RI, August 16–19.

34

http://www.optisyn.com/research/papers/papers/2004/AIAA-2004-5409.pdf .

[7] Uhlmann J. K. Simultaneous map building and localization for real timeapplications. Reporte tecnico, Universidad de Oxford, 1994. Tesis de trans-ferencia.

[8] Wan E. A. & van der Merwe R. Kalman Filtering and NeuralNetworks, chapter 7: The Unscented Kalman Filter. Wiley, 2001.http://citeseer.ist.psu.edu/wan01chapter.html .

[9] Scilab - A Free Scientific Software Package. http://www.scilab.org .

35