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.

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]${\estilo de visualización n}$$Estilo de visualización x_{n}$ ${\estilo de visualización 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: $estilo de visualización {\phi _{n}}$${\estilo de visualización G}$${\estilo de visualización \cdot}$${\displaystyle u_{n}}$${\estilo de visualización 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 : $Estilo de visualización b_{n}$${\displaystyle {\mathcal {Y}}}$${\estilo de visualización G}$${\estilo de visualización *}$${\displaystyle {\mathcal {Y}}}$${\estilo de visualización G}$

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

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. ${\estilo de visualización \exp()}$${\estilo de visualización G}$$Estilo de visualización 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}}$

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]$estilo de visualización f_ {t}}$${\estilo de visualización t}$

${\displaystyle \para todo a,b\en 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. ${\estilo de visualización 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}}$$Estilo de visualización TG_{x}$${\estilo de visualización G}$${\estilo de visualización x}$

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: $Estilo de visualización 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]$Estilo de visualización 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}}$

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.

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 ${\estilo de visualización w,v}$${\estilo de visualización G}$${\estilo de visualización e}$ ${\displaystyle \varphi _ {g}, \ psi _ {g}, \ rho _ {g}}$${\displaystyle g\en 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}$

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}$

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}}$

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})}$

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]