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 invariancias ), ^[1] luego se generalizó y reformuló como una adaptación a los grupos de Lie de la teoría de filtrado de Kalman lineal . ^{[2] [3]} 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 tienen una dependencia reducida del valor estimado del estado. En algunos casos convergen a valores constantes en un conjunto mucho más grande de trayectorias que en el caso del EKF, lo que resulta en una mejor convergencia de la estimación.

Derivación de filtros

Marco de tiempo discreto

Consideremos un sistema cuyo estado está codificado en el paso de tiempo por un elemento de un grupo de Lie y la dinámica tiene la siguiente forma: ^[4] ${\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 se observa a través de 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}*b_{n}}$

donde pertenece a un espacio vectorial dotado de una acción izquierda de los elementos de denotados nuevamente por (lo cual 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 derecha : ${\displaystyle b_{n}}$ ${\displaystyle {\mathcal {Y}}}$ ${\displaystyle G}$ ${\displaystyle *}$ ${\displaystyle {\mathcal {Y}}}$ ${\displaystyle G}$

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

Ecuaciones de filtrado

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}*y_{n}-b_{n}))}$

donde es el mapa exponencial de y es una matriz de ganancia que se ajustará a través de 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}*{\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 tiempo continuo de la forma:

${\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 identidad del grupo se denota por y se utiliza la notación abreviada (resp. ) para la traslación a la izquierda (resp. la traslación a la derecha ) donde denota el espacio tangente a en . Conduce a cálculos más complejos 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 las acciones de izquierda, definimos una variable de error invariante a 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 las 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 las acciones de la 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-1|n-1}^{L})\cdot u_{n}}$,

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

y por las acciones correctas:

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

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

Vemos que el valor estimado del estado no está involucrado en la ecuación seguida de 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 hay términos de orden superior: la dinámica es lineal para la variable de error . Este resultado y la independencia de la dinámica de error son la base 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 los 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, escalas) que dejan el sistema inalterado. Desde un 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}$

donde son ruidos gaussianos blancos independientes . Considérese un grupo de Lie con identidad , y grupos de transformación (locales) ( ) tales que . Se dice que el sistema anterior con ruido es invariante si no cambia con la acción de los grupos de transformación ; 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}$

Ecuaciones de filtrado y resultado principal

Dado que es un filtro que preserva la simetría , la forma general de un IEKF se lee ^[5]

${\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 , que es diferente del error de salida estándar , ya que el error de salida estándar normalmente 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 determinar , 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 ^[6] ${\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í sólo 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 solo alrededor de cualquier punto de equilibrio que varíe lentamente para el EKF. ${\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 dichos sistemas, la orientación, velocidad y/o posición de un cuerpo rígido en movimiento, por ejemplo, una aeronave, se estiman a partir de diferentes sensores integrados, como sensores inerciales, magnetómetros, GPS o sonares. El uso de un IEKF conduce naturalmente ^[6] a considerar el error de 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. ^[7] ${\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 la incorporación del estado (que consiste en la matriz de actitud , el vector de velocidad y el vector de posición ) en el grupo de Lie ^[8] 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áneo

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 la incrustación del estado (que consiste en una matriz de actitud , un vector de posición y una secuencia de puntos de características estáticas ) en el grupo de Lie (o para sistemas planares) ^[8] 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. ^[8]

Referencias

1. Bonnabel, S. (diciembre de 2007). Filtro de Kalman extendido invariante a la izquierda y estimación de actitud. En 2007, 46.ª Conferencia IEEE sobre decisiones y control (pp. 1027-1032). IEEE.
2. Barrau, A., y Bonnabel, S. (2016). El filtro de Kalman extendido invariante como observador estable. IEEE Transactions on Automatic Control, 62(4), 1797-1812.
3. Potokar, Easton R.; Beard, Randal W.; Mangelson, Joshua G. (2024). "Introducción al filtro de Kalman extendido invariante [Apuntes de clase]". IEEE Control Systems . 44 (6): 50–71. doi :10.1109/MCS.2024.3466488. ISSN  1066-033X.
4. Barrau, A., y Bonnabel, S. (2019). Sistemas lineales observados en grupos. Systems & Control Letters, 129, 36-42.
5. 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.
6. 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 velocidad", 48.ª Conferencia IEEE sobre decisiones y control, págs. 1297-1304, 2009.
7. Ph. Martin y E. Salaün, "Filtro Kalman extendido multiplicativo generalizado para el sistema de referencia de rumbo y actitud asistido", Conferencia de guía, navegación y control de la AIAA, 2010
8. Barrau, A., & Bonnabel, S. (2015). Un algoritmo EKF-SLAM con propiedades de consistencia. Preimpresión de arXiv arXiv :1510.06263.