Filtro de Kalman extendido invariante

El filtro de Kalman extendido invariante (IEKF) (que no debe confundirse con el filtro de Kalman extendido iterado) se introdujo por primera vez como una versión del filtro de Kalman extendido (EKF) para sistemas no lineales que poseen simetrías (o invarianzas ), ^[1] luego se generalizó y refundido como una adaptación a los grupos de Lie de la teoría del filtrado lineal de Kalman . ^[2] En lugar de utilizar un término de corrección lineal basado en un error de salida lineal, el IEKF utiliza un término de corrección adaptado geométricamente basado en un error de salida invariante; de la misma manera la matriz de ganancia no se actualiza a partir de un error de estado lineal, sino de un error de estado invariante. El principal beneficio es que las ecuaciones de ganancia y covarianza han reducido la dependencia del valor estimado del estado. En algunos casos convergen a valores constantes en un conjunto de trayectorias mucho mayor que en el caso del EKF, lo que resulta en una mejor convergencia de la estimación.

Derivación de filtro

Marco de tiempo discreto

Considere un sistema cuyo estado está codificado en un paso de tiempo por un elemento de un grupo de Lie y la dinámica tiene la siguiente forma: ^[3] ${\displaystyle n}$ ${\displaystyle x_{n}}$ ${\displaystyle G}$

${\displaystyle x_{n}=\phi _{n}(x_{n-1})\cdot u_{n}}$

donde es un automorfismo de grupo de , es la operación de grupo y un elemento de . Se supone que el sistema debe observarse mediante una medición que tiene la siguiente forma: ${\displaystyle \phi _{n}}$ ${\displaystyle G}$ ${\displaystyle \cdot }$ ${\displaystyle u_{n}}$ ${\displaystyle G}$ ${\displaystyle y_{n}}$

${\displaystyle y_{n}=x_{n}\cdot b_{n}}$

donde pertenece a un espacio vectorial dotado de una acción izquierda de los elementos de denotado nuevamente por (lo que no puede crear confusión con la operación de grupo ya que el segundo miembro de la operación es un elemento de , no ). Alternativamente, la misma teoría se aplica a una medida definida por una acción correcta : ${\displaystyle b_{n}}$ ${\displaystyle {\mathcal {Y}}}$ ${\displaystyle G}$ ${\displaystyle \cdot }$ ${\displaystyle {\mathcal {Y}}}$ ${\displaystyle G}$

${\displaystyle y_{n}=b_{n}\cdot x_{n}}$

Filtrar ecuaciones

El filtro de Kalman extendido invariante es un observador definido por las siguientes ecuaciones si la función de medición es una acción izquierda: ${\displaystyle {\hat {x}}_{n}}$

${\displaystyle {\hat {x}}_{n|n-1}=\phi _{n}({\hat {x}}_{n-1|n-1})\cdot u_{n}}$

${\displaystyle {\hat {x}}_{n|n}={\hat {x}}_{n|n-1}\cdot \exp(K_{n}({\hat {x}}_{n|n-1}^{-1}\cdot y_{n}-b_{n}))}$

donde es el mapa exponencial de y es una matriz de ganancia que se sintonizará mediante una ecuación de Riccati. ${\displaystyle \exp()}$ ${\displaystyle G}$ ${\displaystyle K_{n}}$

Si la función de medición es una acción correcta, entonces el estado de actualización se define como:

${\displaystyle {\hat {x}}_{n|n}=\exp(K_{n}(y_{n}\cdot {\hat {x}}_{n|n-1}^{-1}-b_{n}))\cdot {\hat {x}}_{n|n-1}}$

Marco de tiempo continuo

El marco de tiempo discreto anterior se introdujo por primera vez para la dinámica de la forma en tiempo continuo:

${\displaystyle {\frac {d}{dt}}x_{t}=f_{t}(x_{t}),}$

donde el campo vectorial verifica en cualquier momento la relación: ^[2] ${\displaystyle f_{t}}$ ${\displaystyle t}$

${\displaystyle \forall a,b\in G,f_{t}(a\cdot b)=f_{t}(a)b+af_{t}(b)-af(Id)b}$

donde el elemento de identidad del grupo se denota por y se utiliza la notación abreviada (resp. ) para la traducción a la izquierda (resp. la traducción a la derecha ) donde denota el espacio tangente a en . Conduce a cálculos más complicados que el marco de tiempo discreto, pero las propiedades son similares. ${\displaystyle Id}$ ${\displaystyle gu=L_{g}(u)}$ ${\displaystyle ug=R_{g}(u)}$ ${\displaystyle L_{g}:TG_{x}\rightarrow TG_{gx}}$ ${\displaystyle R_{g}:TG_{x}\rightarrow TG_{xg}}$ ${\displaystyle TG_{x}}$ ${\displaystyle G}$ ${\displaystyle x}$

Propiedades principales

El principal beneficio del filtrado de Kalman extendido invariante es el comportamiento de la variable de error invariante, cuya definición depende del tipo de medición. Para acciones por la izquierda definimos una variable de error invariante por la izquierda como:

${\displaystyle e_{n|n-1}^{L}={\hat {x}}_{n|n-1}^{-1}x_{n}}$,

${\displaystyle e_{n|n}^{L}={\hat {x}}_{n|n}^{-1}x_{n}}$,

mientras que para acciones correctas definimos una variable de error invariante a la derecha como:

${\displaystyle e_{n|n-1}^{R}=x_{n}{\hat {x}}_{n|n-1}^{-1}}$,

${\displaystyle e_{n|n}^{R}=x_{n}{\hat {x}}_{n|n}^{-1}}$,

De hecho, reemplazando , , por sus valores obtenemos para acciones de izquierda, después de algo de álgebra: ${\displaystyle x_{n}}$ ${\displaystyle {\hat {x}}_{n|n-1}}$ ${\displaystyle {\hat {x}}_{n|n}}$

${\displaystyle e_{n|n-1}^{L}=u_{n}^{-1}\cdot \phi _{n}(e_{n|n-1}^{L})\cdot u_{n}}$,

${\displaystyle e_{n|n}^{L}=\exp(-K_{n}(e_{n|n-1}^{L}\cdot b_{n}-b_{n}))\cdot e_{n|n-1}^{L}}$,

y para acciones correctas:

${\displaystyle e_{n|n-1}^{R}=\phi _{n}(e_{n|n-1}^{R})}$,

${\displaystyle e_{n|n}^{R}=e_{n|n-1}^{R}\exp(-K_{n}(b_{n}\cdot e_{n|n-1}^{R}-b_{n}))}$,

Vemos que el valor estimado del estado no está involucrado en la ecuación seguida por la variable de error, una propiedad del filtrado de Kalman lineal que el filtro de Kalman extendido clásico no comparte, pero la similitud con el caso lineal en realidad va mucho más allá. Sea una versión lineal de la variable de error definida por la identidad: ${\displaystyle \xi _{n|n-1},\xi _{n|n}}$

${\displaystyle e_{n|n-1}=\exp(\xi _{n|n-1}),}$

${\displaystyle e_{n|n}=\exp(\xi _{n|n}).}$

Entonces, definido por la expansión de Taylor, en realidad tenemos: ^[2] ${\displaystyle F_{n}}$ ${\displaystyle \xi _{n|n}=F_{n}\xi _{n|n-1}+\circ (||\xi _{n|n-1}||)}$

${\displaystyle \xi _{n|n}=F_{n}\xi _{n|n-1}.}$

En otras palabras, no existen términos de orden superior: la dinámica es lineal para la variable de error . Este resultado y la independencia de la dinámica del error son el núcleo de las propiedades teóricas y el desempeño práctico de IEKF. ^[2] ${\displaystyle \xi _{n|n},\xi _{n|n-1}}$

Relación con observadores que preservan la simetría

La mayoría de los sistemas físicos poseen simetrías naturales (o invariancia), es decir, existen transformaciones (por ejemplo, rotaciones, traslaciones, escalamientos) que dejan el sistema sin cambios. Desde el punto de vista matemático y de ingeniería, tiene sentido que un filtro bien diseñado para el sistema considerado conserve las mismas propiedades de invariancia. La idea del IEKF es una modificación de las ecuaciones del EKF para aprovechar las simetrías del sistema.

Definición

Considere el sistema

${\displaystyle {\dot {x}}{=}f(x,u)+M(x)w}$

${\displaystyle y{=}h(x,u)+N(x)v}$

¿Dónde están los ruidos blancos gaussianos independientes ? Considere un grupo de Lie con identidad y grupos de transformación (locales) ( ) tales que . El sistema anterior con ruido se dice invariante si no se modifica por la acción de los grupos de transformaciones ; es decir, si ${\displaystyle w,v}$ ${\displaystyle G}$ ${\displaystyle e}$ ${\displaystyle \varphi _{g},\psi _{g},\rho _{g}}$ ${\displaystyle g\in G}$ ${\displaystyle (X,U,Y)=(\varphi _{g}(x),\psi _{g}(u),\rho _{g}(y))}$ ${\displaystyle \varphi _{g},\psi _{g},\rho _{g}}$

${\displaystyle {\dot {X}}{=}f(X,U)+M(X)w}$.

${\displaystyle Y{=}h(X,U)+N(X)v}$

Filtrar ecuaciones y resultado principal.

Dado que es un filtro que preserva la simetría , la forma general de un IEKF dice ^[4]

${\displaystyle {\dot {\hat {x}}}=f({\hat {x}},u)+W({\hat {x}})L{\Bigl (}I({\hat {x}},u),E({\hat {x}},u,y){\Bigr )}E({\hat {x}},u,y)}$

dónde

• ${\displaystyle E({\hat {x}},u,y)}$es un error de salida invariante, que es diferente del error de salida habitual ${\displaystyle {\hat {y}}-y}$
• ${\displaystyle W({\hat {x}})={\bigl (}w_{1}({\hat {x}}),..,w_{n}({\hat {x}}){\bigr )}}$es un marco invariante
• ${\displaystyle I({\hat {x}},u)}$es un vector invariante
• ${\displaystyle L(I,E)}$es una matriz de ganancia elegida libremente.

Para analizar la convergencia del error se define un error de estado invariante, el cual es diferente del error estándar de salida , ya que el error estándar de salida generalmente no preserva las simetrías del sistema. ${\displaystyle \eta ({\hat {x}},x)}$ ${\displaystyle {\hat {x}}-x}$

Dado el sistema considerado y el grupo de transformación asociado, existe un método constructivo para determinarlo , basado en el método del marco móvil. ${\displaystyle E({\hat {x}},u,y),W({\hat {x}}),I({\hat {x}},u),\eta ({\hat {x}},x)}$

De manera similar al EKF, la matriz de ganancia se determina a partir de las ecuaciones ^[5] ${\displaystyle L(I,E)}$

${\displaystyle L{=}PC^{T}{\bigl (}N(e)N^{T}(e){\bigr )}^{-1}}$,

${\displaystyle {\dot {P}}{=}AP+PA^{T}+M(e)M^{T}(e)-PC^{T}{\bigl (}N(e)N^{T}(e){\bigr )}^{-1}CP}$,

donde las matrices dependen aquí solo del vector invariante conocido , en lugar de como en el EKF estándar. Esta dependencia mucho más simple y sus consecuencias son los principales intereses del IEKF. De hecho, las matrices son entonces constantes en un conjunto mucho mayor de trayectorias (las llamadas trayectorias permanentes ) que los puntos de equilibrio, como es el caso del EKF. Cerca de tales trayectorias, volvemos al filtro de Kalman "verdadero", es decir, lineal, donde la convergencia está garantizada. Informalmente, esto significa que el IEKF converge en general al menos alrededor de cualquier trayectoria permanente que varíe lentamente, en lugar de simplemente alrededor de cualquier punto de equilibrio del EKF que varíe lentamente. ${\displaystyle A,C}$ ${\displaystyle I({\hat {x}},u)}$ ${\displaystyle ({\hat {x}},u)}$ ${\displaystyle A,C}$

Ejemplos de aplicación

Sistemas de referencia de actitud y rumbo.

Los filtros de Kalman extendidos invariantes se utilizan, por ejemplo, en sistemas de referencia de actitud y rumbo . En tales sistemas, la orientación, velocidad y/o posición de un cuerpo rígido en movimiento, por ejemplo un avión, se estiman a partir de diferentes sensores integrados, como sensores inerciales, magnetómetros, GPS o sonares. El uso de un IEKF lleva naturalmente a ^[5] a considerar el error del cuaternión , que a menudo se utiliza como un truco ad hoc para preservar las restricciones del grupo de cuaterniones. Los beneficios del IEKF en comparación con el EKF se muestran experimentalmente para un gran conjunto de trayectorias. ^[6] ${\displaystyle {\hat {q}}q^{-1}}$

Navegación inercial

Una aplicación importante del filtro de Kalman extendido invariante es la navegación inercial , que se ajusta al marco después de incrustar el estado (que consta de una matriz de actitud , un vector de velocidad y un vector de posición ) en el grupo de Lie ^[7] definido por la operación de grupo: ${\displaystyle R}$ ${\displaystyle v}$ ${\displaystyle x}$ ${\displaystyle SE_{2}(3)}$

${\displaystyle (R_{1},v_{1},x_{1})\cdot (R_{2},v_{2},x_{2})=(R_{1}R_{2},x_{1}+R_{1}x_{2},v_{1}+R_{1}v_{2})}$

Localización y mapeo simultáneos

El problema de la localización y el mapeo simultáneos también se ajusta al marco del filtrado de Kalman extendido invariante después de incorporar el estado (que consta de una matriz de actitud , un vector de posición y una secuencia de puntos característicos estáticos ) en el grupo de Lie (o para sistemas planos) ^[7] definido por la operación de grupo: ${\displaystyle R}$ ${\displaystyle x}$ ${\displaystyle p^{1},\dots ,p^{K}}$ ${\displaystyle SE_{K+1}(3)}$ ${\displaystyle SE_{K+1}(2)}$

${\displaystyle (R_{1},x_{1},p_{1}^{1},\dots ,p_{1}^{K})\cdot (R_{2},x_{2},p_{2}^{2},\dots ,p_{2}^{K})=(R_{1}R_{2},x_{1}+R_{1}x_{2},p_{1}^{1}+R_{1}p_{2}^{1},\dots ,p_{1}^{K}+R_{1}p_{2}^{K})}$

El principal beneficio del filtro de Kalman extendido invariante en este caso es resolver el problema de la falsa observabilidad. ^[7]

Referencias

1. Bonnabel, S. (2007, diciembre). Filtro de Kalman extendido invariante a la izquierda y estimación de actitud. En 2007, 46.ª Conferencia del IEEE sobre Decisión y Control (págs. 1027-1032). IEEE.
2. Barrau, A. y Bonnabel, S. (2016). El filtro de Kalman extendido invariante como observador estable. Transacciones IEEE sobre control automático, 62(4), 1797-1812.
3. Barrau, A. y Bonnabel, S. (2019). Sistemas lineales observados sobre grupos. Cartas de sistemas y control, 129, 36-42.
4. S. Bonnabel, Ph. Martin y P. Rouchon, “Observadores que preservan la simetría”, IEEE Transactions on Automatic and Control , vol. 53, núm. 11, págs. 2514-2526, 2008.
5. S. Bonnabel, Ph. Martin y E. Salaün, "Filtro de Kalman extendido invariante: teoría y aplicación a un problema de estimación de actitud asistida por la velocidad", 48ª Conferencia IEEE sobre Decisión y Control, págs.
6. Ph. Martin y E. Salaün, "Filtro de Kalman extendido multiplicativo generalizado para sistema de referencia de rumbo y actitud asistida", Conferencia de control, navegación y orientación de la AIAA, 2010
7. Barrau, A. y Bonnabel, S. (2015). Un algoritmo EKF-SLAM con propiedades de coherencia. arXiv preimpresión arXiv :1510.06263.