9.5: Filtro extendido de Kalman

En el filtro extendido de Kalman, los modelos de transición de estado y observación no necesitan ser funciones lineales del estado sino que pueden ser funciones diferenciables. El paso de actualización de acción se ve de la siguiente manera:

\[\textbf{x̂}_{k'|k-1}=f(\textbf{x̂}_{k-1|k-1},\textbf{u}_{k-})\]

Aquí f () es una función del estado antiguo x _k−1 y la entrada de control _uk−1. Esto no es otra cosa como la actualización de odometría a la que estamos acostumbrados, donde f () es una función que describe la cinemática hacia delante del robot, x _k su posición y u _k la velocidad de rueda que establecemos.

También podemos calcular la matriz de covarianza de la posición del robot

\[\mathbf{P}_{k'|k-1}=∇_{x,y,\theta}f\mathbf{P}_{k-1|k-1}∇_{x,y,\theta }f^{T}+∇_{\Delta _{r,l}}f\mathbf{Q}_{k-1}∇_{\Delta _{r,l}f}\]

Esto no es otra cosa como la ley de propagación de errores aplicada a la odometría del robot con Q _k la matriz de covarianza del deslizamiento de rueda y las matrices jacobianas de las ecuaciones cinemáticas hacia adelante f () con respecto a la posición del robot (indicada por el índice x, y, θ) y con respecto al deslizamiento de la rueda de la rueda izquierda y derecha.

El paso de actualización de percepción (o predicción) se ve de la siguiente manera:

\[\boldsymbol{x̂}_{k|k'}=\boldsymbol{x̂}_{k'|k-1}+\boldsymbol{K}_{k'}\boldsymbol{y}_{k'}\]

\[\boldsymbol{P}_{k|k'}=(I-\boldsymbol{K}_{k'}\boldsymbol{H}_{k'})\boldsymbol{P}_{k'|k-1}\]

En este punto los índices k deberían empezar a tener sentido. Estamos calculando todo dos veces: una vez que actualizamos de k − 1 a un resultado intermedio k' durante la actualización de acción, y obtenemos el resultado final después de la actualización de percepción donde pasamos de k' a k.

Necesitamos calcular tres variables adicionales:

1. La innovación y~ _k = z _k − h (_xk|k−1)
2. La covarianza de la innovación S _k = H _k P _k|k−1 H ^T _k + R _k
3. La ganancia de Kalman (casi óptima) K _k = P _k|k−1 H ^T _k S ⁻¹ _k

Aquí h () es el modelo de observación y H su jacobiano. La forma en que se derivan estas ecuaciones está implicada (y es uno de los resultados fundamentales en la teoría del control), pero la idea es la misma que la introducida anteriormente: deseamos minimizar el error de la predicción.

9.5.1. Odometría con el Filtro Kalman

Mostraremos cómo un robot móvil equipado con un escáner láser puede corregir su estimación de posición confiando en una odometría poco confiable, detección poco confiable, pero un mapa correcto, de manera óptima. Mientras que el paso de actualización es equivalente a la cinemática directa y la propagación de errores que hemos visto antes, el modelo de observación y la “innovación” requieren pasos adicionales para realizar la odometría.

1. Actualización de predicción: Suponemos por ahora que el lector está familiarizado con el cálculo de _{x^k' |k−1} = f (x, y, θ) ^T y su varianza P _k'|k−1. Aquí, Q _k−1, la matriz de covarianza del error de deslizamiento de rueda, viene dada por

\ [Q_ {k-1} =\ begin {bmatrix}
k_ {r} |\ Delta s_ {r} & 0\\
0 & k_ {l} |\ Delta s_ {l}
\ end {bmatrix}\]

donde ∆s es el movimiento de rueda de la rueda izquierda y derecha y k son constantes. Consulte también el laboratorio de odometría para obtener derivaciones detalladas de estos cálculos y cómo estimar k _r y k _l. El vector de estado _{x^k' |k−1} es un vector 3x1, la matriz de covarianza _Pk'|k−1 es una matriz 3x3, y _{∆r, l} que se usa durante la propagación de errores es una matriz de 3x2. Consulte la sección de propagación de errores para obtener detalles sobre cómo calcular _{∆r, l}.

2. Observación: Supongamos ahora que podemos detectar entidades de línea z _{k, i} = (α _i, r _i) ^T, donde α y r son el ángulo y la distancia de la línea desde el sistema de coordenadas del robot. Estas características de línea están sujetas a varianzas σ _{α, i} y σ _{r, i}, que conforman la diagonal de _Rk. Consulte la sección de detección de líneas para obtener una derivación de cómo se pueden calcular el ángulo y la distancia, así como su varianza a partir de un escáner láser. La observación es una matriz 2x1.

3. Predicción de medición: Asumimos que podemos identificar de manera única las líneas que estamos viendo y recuperar su posición real de un mapa. Esto es mucho más fácil para características únicas, pero se puede hacer también para líneas asumiendo que nuestro error es lo suficientemente pequeño y por lo tanto podemos buscar a través de nuestro mapa y elegir las líneas más cercanas. Como las características se almacenan en coordenadas globales, necesitamos transponerlas en la forma en que el robot las vería. En la práctica esto no es más que una lista de líneas, cada una con un ángulo y una distancia, pero esta vez con respecto al origen del sistema de coordenadas global. Transponerlos en coordenadas de robot es sencillo. Con _xk = (x _k, y _k, θ _k) ^T y m _i = (α _i, r _i) la entrada correspondiente del mapa, podemos escribir

\ [h (\ negridsymbol {x} _ _ {k|k-1}) =\ begin {bmatrix}
\ alpha _ {k, i}\\
r _ {k, i}
\ end {bmatrix} =h (\ negridsymbol {x}, m_ {i}) =\ begin {bmatrix}
\ alpha _ {i} -\ theta\
r_ {i} - (cos (\ alpha _ {i}) +ysin (\ alpha _ {i}))
\ end {bmatrix}\]

y calcular su jacobiano H _k como las derivadas parciales de α a x, y, θ en la primera fila, y las derivadas parciales de r en la segunda. Cómo calcular h () para predecir el radio en el que el robot debería ver la entidad con radio ri del mapa se ilustra en la siguiente figura.

• Ejemplo sobre cómo predecir la distancia a una entidad que vería el robot dada su posición estimada y su ubicación conocida a partir de un mapa.

4. Coincidencia: Ahora estamos equipados con una medida z _k y una predicción h (_{xk |k−1}) basada en todas las características almacenadas en nuestro mapa. Ahora podemos calcular la innovación

\[\boldsymbol{y}_{k}=\boldsymbol{z}_{k}-h(\boldsymbol{x̂}_{k|k-1})\]

5. Estimación: Ahora tenemos todos los ingredientes para realizar el paso de actualización de percepción del filtro Kalman:

\[\boldsymbol{x̂}_{k'|k-1}=\boldsymbol{x̂}_{k'|k-1}+\boldsymbol{K}_{k'}\boldsymbol{y}_{k'}\]

\[\boldsymbol{P}_{k|k'}=(I-\boldsymbol{K}_{k'}\boldsymbol{H}_{k'})\boldsymbol{P}_{k'|k-1}\]

Nos proporcionará una actualización de nuestra posición que fusiona nuestra entrada de odometría e información que podemos extraer de las características del entorno de una manera que tenga en cuenta sus varianzas. Es decir, si la varianza de tu posición anterior es alta (porque no tienes idea de dónde estás), pero la varianza de tu medición es baja (tal vez desde un GPS o un símbolo en la pared Ratslife), el filtro Kalman pondrá más énfasis en tu sensor. Si tus sensores son malos (tal vez porque no puedes distinguir diferentes líneas/paredes separadas), más énfasis estará en la odometría.

Como el vector de estado es un vector 3x1 y la innovación una matriz 2x1, la ganancia de Kalman debe ser una matriz de 3x2. Esto también se puede ver al mirar la matriz de covarianza que debe salir como una matriz 3x3, y sabiendo que el jacobiano de la función de observación es una matriz 2x3. Ahora podemos calcular la covarianza de la innovación y la ganancia de Kalman usando

\[\boldsymbol{S}_{k}=\boldsymbol{H}_{k}\boldsymbol{P}_{k|k-1}\boldsymbol{H}_{k}^{T}+\boldsymbol{R}_{k}\]

\[\boldsymbol{K}_{k}=\boldsymbol{P}_{k|k-1}\boldsymbol{H}_{k}^{T}\boldsymbol{S}_{k}^{-1}\]