EKF y UKF - Facultad de Ingeniería

Modelos de estado no-lineales: EKF y UKF
Tratamiento Estadístico de Señales
Pablo Musé, Ernesto López, Luis Di Martino
{pmuse,elopez,dimartino}@fing.edu.uy
Departamento de Procesamiento de Señales
Instituto de Ingeniería Eléctrica
Facultad de Ingeniería
Junio de 2015
Biblio:
I Anderson and Moore, Optimal Filtering, 1979 (re-edición Dover 2005).
I Julier, S.J.; Uhlmann, J.K., Unscented filtering and nonlinear estimation,
Proceedings of the IEEE , vol.92, no.3, pp.401,422, Mar 2004.
Introducción - Motivación
Consideremos los modelos de estado en sus versiones general y lineal:
xk+1 = fk+1 (xk , vk+1 )
xk+1 = Fk+1 xk + Gk+1 vk+1
yk
= hk (xk , nk )
yk
= Hk xk + Uk nk
Hasta ahora hemos visto sobre el filtro de Kalman, que:
I
I
I
Si los ruidos no son necesariamente Gaussianos, el filtro de Kalman
es óptimo entre todos los filtros lineales, en el sentido del MSE.
Si los ruidos son Gaussianos, el filtro de Kalman es universalmente
óptimo (i.e. aún dentro del conjunto de los filtros no-lineales) en el
sentido del MSE.
Todo proceso ARMA se puede representar como un filtro de
Kalman, pasando a una representación en variables de estado.
Hoy nos concentraremos en dos técnicas para modelos no-lineales:
1. Extended Kalman Filter (EKF): básicamente una linealización del
modelo y una aplicación del Kalman clásico.
2. Unscented Kalman Filter (UKF): básicamente, mapear por las
transformaciones no-lineales un conjunto de puntos bien elegidos, y
a partir de ellos calcular nuevas medias y matrices de covarianzas.
El filtro de Kalman Extendido (EKF)
La linealización
Consideramos ahora un modelo en el espacio de estados no-lineal, de la
forma:
xk+1 = fk+1 (xk , vk+1 )
yk
= hk (xk , nk ).
El filtro de Kalman extendido se basa en una aproximación de primer
orden del modelo, seguido del loop de Kalman con ecuaciones lineales.
I
La ecuación de estado se linealiza entorno a (x, v) = (xk|k , 0).
I
La ecuación de medida se linealiza entorno a (x, n) = (xk|k−1 , 0).
xk+1
yk
= fk+1 (xk|k , 0) + Fk+1 (xk+1 − xk|k ) + Gk+1 vk+1
= hk (xk|k−1 , 0) + Hk (xk − xk|k−1 ) + Uk nk ,
dónde
∂fk+1
(xk|k , 0),
∂x
∂hk
Hk =
(xk|k−1 , 0),
∂x
Fk+1 =
∂fk+1
(xk|k , 0),
∂v
∂hk
Uk =
(xk|k−1 , 0).
∂n
Gk+1 =
El filtro de Kalman Extendido (EKF)
Las iteraciones de Kalman
Las ecuaciones para el EKF quedan entonces:
I
Inicialización: x0 , P0 .
I
Para k = 0, 1, 2, . . . , calcular:
x̂k|k−1 = fk (x̂k−1|k−1 , 0)
Pk|k−1 = Fk Pk−1|k−1 FTk + Gk Qk GTk
Kk = Pk|k−1 HTk (Hk Pk|k−1 HTk + Uk Rk UTk )−1
x̂k|k = x̂k|k−1 + Kk (yk − hk (x̂k|k−1 , 0))
Pk|k = (I − Kk Hk )Pk|k−1
Observaciones y comentarios:
I El problema del filtro EKF es que la linealización del modelo
introduce un sesgo en x̂k|k y una covarianza errónea.
I La calidad de la aproximación debida a la linealización será mejor
cuanto menor sean kx − x̂k|k k2 y kx − x̂k|k−1 k2 .
I Por ende, es de esperar que el EKF se comporte mejor en situaciones
de alto SNR. Ver [Anderson & Moore] para una discusión más
profunda sobre el tema.
El Unscented Kalman Filter (UKF)
Julier & Uhlmann 1996
Este método busca dejar de lado las aproximaciones y trabajar
directamente sobre las transformaciones no lineales, mapeando un
conjunto de puntos bien elegidos.
La transformación unscented
I
Sea x ∼ N (x̄, Px ) un vector aleatorio de Rn , g : Rn → Rm una
función, e y = g(x) ∈ Rm .
I
La unscented transform busca calcular estimadores insesgados de ȳ
y de Py .
I
En lugar de linealizar g como en el EKF, la propuesta es evaluar ȳ y
Py (1er y 2do momento de y) mediante estimaciones empíricas
ponderadas, calculadas a partir de la transformación exacta g de un
conjunto de puntos bien elegidos alrededor de x̄, denominados
σ-points.
UKF: Los σ-points
Tenemos x ∈ Rn , x ∼ N (x̄, Px ).
Los 2n + 1 σ-points y sus pesos correspondientes se definen como:
κ
κ+n
1
Wi =
, i = 1, . . . , n
2(κ + n)
1
Wi =
, i = n + 1, . . . , 2n.
2(κ + n)
X0 = x̄
W0 =
Xi = x̄ + (
p
(κ + n)Px )i
p
Xi = x̄ − ( (κ + n)Px )i−n
√
donde κ es un escalar y ( Px )i representa a la i-ésima columna de
alguna factorización de Px , e.g. Cholesky, i.e.
Px =
p
Px ×
p
T
Px =
n p
X
p
( Px )i ( Px )Ti .
i=1
En lo que sigue veremos porqué Julier & Uhlmann (1996) proponen esta
elección.
Justificación de la elección de los σ-points
A partir de los σ-points y sus transformados, se puede calcular
estadísticos empíricos de x y de y. Sea Yi = g(Xi ), definimos
=
P2n
Wi X i
PX
=
P2n
Wi (Xi − X̄ )(Xi − X̄ )T
Ȳ
=
P2n
Wi Y i
=
P2n
Wi (Yi − Ȳ)(Yi − Ȳ)T .
X̄
PY
i=0
i=0
i=0
i=0
Ejercicio: demostrar que la media y covarianza empíricas de
X = {Xi }2n
i=0 coinciden con las de x.
Claramente, X̄ = x̄.
√
√
Pn
1
Luego, PX = 2(n+κ)
× 2 i=1 (n + κ)( Px )i ( Px )Ti = Px .
Justificación de la elección de los σ-points (cont.)
Resta ver qué pasa con la media y covarianza empíricas de los {Yi }2n+1
i=1
en relación a y.
Suponemos que g = [g1 , g2 , . . . , gm ]T , g : Rn → Rm , admite un
desarrollo de Taylor entorno a x̄. Entonces tenemos
1
g(x̄ + δ) = g(x̄) + Dδ g(x̄) + Dδ2 g(x̄) + . . . ,
2
dónde
Dδ g(x̄) = ∇T g(x̄)δ
Dδ2 g(x̄) = Dδ [∇g(x̄)δ] = ∇T [∇T g(x̄)δ]δ
= [Tr(∇2 g1 (x̄)δδ T ), . . . , Tr(∇2 gm (x̄)δδ T )]T ,
con ∇g la matriz Jacobiana de g, y ∇2 g la matriz Hessiana de g.
Justificación de la elección de los σ-points (cont.)
Luego,
ȳ = E[g(x)] = E[g(x̄ + δ)]
= g(x̄) + E[∇T g(x̄)δ] +
1
E[[Tr(∇2 g1 (x̄)δδ T ), . . . , Tr(∇2 gm (x̄)δδ T )]T ] + . . .
2
Como x = x̄ + δ, tenemos δ ∼ N (0, Px ), por lo que
1
[Tr(∇2 g1 (x̄)Px ), . . . , Tr(∇2 gm (x̄)Px )]T + . . .
2
p
Por otra parte, considerando σ i = ( (κ + n)Px )i ,
ȳ = g(x̄) +
Ȳ = W0 g(x̄) +
n
X
Wi (g(x̄ + σ i ) + g(x̄ − σ i ))
i=1
= g(x̄) + 0 +
= g(x̄) +
2n
2n
X
X
σi σT
σi σT
1
i
i
E[[Tr(∇2 g1 (x̄)
, . . . , Tr(∇2 gm (x̄)
)]T ] + . . .
2
2(κ
+
n)
2(κ
+ n)
i=1
i=1
1
[Tr(∇2 g1 (x̄)Px ), . . . , Tr(∇2 gm (x̄)Px )]T + . . .
2
En ambos casos, debido a la simetría de los σ-points entorno a x̄, los
términos impares se anulan, por los que la media empírica de los {Yi }2n
i=0
coincide con ȳ hasta el tercer orden.
Justificación de la elección de los σ-points (cont.)
Ahora vemos qué pasa con Py y PY .
Py = E[(g(x̄ + δ) − g(x̄))(g(x̄ + δ) − g(x̄))T ]
= E[(∇T g(x̄)δ)(∇T g(x̄)δ)T ] + . . .
= ∇T g(x̄)Px ∇g(x̄) + . . .
Por otro lado,
P2n
PY = i=0 Wi (Yi − Ȳ)(Yi − Ȳ)T
Pn
1
= 2(n+κ)
× 2 i=1 (∇T g(x̄)σ i + . . . )(∇T g(x̄)σ i + . . . )T
= ∇T g(x̄)Px ∇g(x̄) + . . .
Por los que ambas coinciden en el primer orden. Ajustando κ para que
n + κ = 3, se puede mostrar que las medias pueden coincidir hasta orden
4. Sin embargo, la covarianza empírica ponderada puede volverse
semidefinida negativa. Este problema fue resuelto por Julier en 2002,
introduciendo la scaled unscented transformation.
Scaled Unscented Transform
I
La idea es escalar la distancia entre los σ-points remplazando Xi por
Xi0 = X0 + α(Xi − X0 ), con α el factor de escala.
I
En este caso W0 debe ser diferente. Llamando λ = α2 (κ + n) + n, la
scaled unscented transform se resume así:
X0 = x̄
λ
,
λ+n
(m)
=
(m)
=Wi
(m)
=Wi
W0
Xi = x̄ + (
p
(λ + n)Px )i
Wi
Xi = x̄ − (
p
(λ + n)Px )i−n
Wi
(c)
(c)
(c)
W0
=
λ
+ (1 − α2 + β)
λ+n
1
, i = 1, . . . , n
2(κ + n)
1
=
, i = n + 1, . . . , 2n,
2(κ + n)
=
donde 0 ≤ α ≤ 1, κ puede ser seteado a 0, y β = 2 es el valor
óptimo en el caso Gaussiano.
De esta forma, las medias y covarianzas empíricas de x e y quedan:
P2n
(m)
X̄
= i=0 Wi Xi
P2n
(c)
PX = i=0 Wi (Xi − X̄ )(Xi − X̄ )T
P2n
(m)
Ȳ
= i=0 Wi Yi
P2n
(c)
PY = i=0 Wi (Yi − Ȳ)(Yi − Ȳ)T
Unscented Kalman Filter (UKF)
El UKF se obtiene juntando las formulas del filtro de Kalman con las de
la Unscented Transform. Considerando nuevamente el modelo no-lineal
xk+1 = fk+1 (xk , vk+1 )
yk
= hk (xk , nk ),
y usando una representación de estado aumentada xak = [xTk vkT nTk ]T
con su correspondiente matriz de covarianza, el UKF se resume como:
1. Inicialización: x0|0 , P0|0
2. Iteraciones: para k = 1, 2, . . . ,

a
P
=
Pk−1|k−1
0
0
0
Qk
0

0
0 
Rk
x
Xk|k−1


x
Xk−1|k−1
h
i
p
a
v
a = 

= xa
|
x
±
(n
+
λ)P
Xk
k−1|k−1
k−1|k−1
Xkn
x
= fk (Xk−1|k−1
, Xkv )
xk|k−1
=
P2n
Wi
Px
k|k−1
=
P2n
Wi
Yk|k−1
x
= h(Xk|k−1
, Xkn )
yk|k−1
=
P2n
Py
k|k−1
=
P2n
Pxy
k|k−1
=
P2n
Kk
= Pxy
(Py
)−1
k|k−1
k|k−1
xk|k
= xk|k−1 + Kk (yk − yk|k−1 )
Pk|k
= Pk|k−1 − Kk Py
KT
k.
k|k−1
Xa
i=0
i=0
i=0
(m)
(c)
x
x
(Xk|k−1,i
− xk|k−1 )(Xk|k−1,i
− xk|k−1 )T
(m)
Wi
x
Xk|k−1,i
Yk|k−1,i
(c)
(Yk|k−1,i
i=0 Wi
i=0
(c)
Wi
− yk|k−1 )(Yk|k−1,i − yk|k−1 )T
x
(Xk|k−1,i
− xk|k−1 )(Yk|k−1,i − yk|k−1 )T