El filtro de Kalman y sus variantes

MT3006 - Robótica 2

¿Por qué no es suficiente el filtro complementario?

Necesitamos una herramienta que lidie directamente con la naturaleza estocástica del problema

Un flashback a control moderno

\mathbf{B}
\boldsymbol{+}
\boldsymbol{\int}
\mathbf{C}
\mathbf{A}
\mathbf{u}
\mathbf{y}
\dot{\mathbf{x}}
\mathbf{x}
\mathbf{K}
\mathbf{x}
(-1)

controlador lineal

state feedback

retroalimentación negativa

\boldsymbol{???}

observador de estado

\hat{\mathbf{x}} \approx \mathbf{x}

¿Qué es?

¿Cuándo funciona?

(valor) estimado del estado

Un flashback a control moderno

\mathbf{y}
\hat{\mathbf{x}} \approx \mathbf{x}

es un estimador

???

sistema que genera estimados | estimaciones

\triangleq

Según Luenberger

\dot{\hat{\mathbf{x}}}=\mathbf{A}\hat{\mathbf{x}}+\mathbf{B}\mathbf{u}+\mathbf{L}\left(\mathbf{y}-\hat{\mathbf{y}}\right)

Un flashback a control moderno

Observadores de Luenberger

\dot{\hat{\mathbf{x}}}=\mathbf{A}\hat{\mathbf{x}}+\mathbf{B}\mathbf{u}+\mathbf{L}\left(\mathbf{y}-\hat{\mathbf{y}}\right)

predicción

emplea lo que se conoce del sistema

corrección

emplea las mediciones para corregir la predicción

matriz de ganancias \(\in\mathbb{R}^{n \times p}\)

mediciones

salida según el modelo LTI \(\hat{\mathbf{y}}=\mathbf{C}\hat{\mathbf{x}}\)

Pero existe otra manera de interpretar el problema, desde una perspectiva de estimación

¿Estimación?

considera dos problemas

P\left( \mathbf{x}_{1:N},\theta | \mathbf{y}_{1:N} \right)
P\left( \mathbf{x}_t | \mathbf{y}_{1:t} \right)

considera dos problemas

\hat{\mathbf{x}}
P\left( \mathbf{x}_{1:N},\theta | \mathbf{y}_{1:N} \right)
P\left( \mathbf{x}_t | \mathbf{y}_{1:t} \right)

smoothing

¿Estimación?

considera dos problemas

\hat{\mathbf{x}}
P\left( \mathbf{x}_{1:N},\theta | \mathbf{y}_{1:N} \right)
P\left( \mathbf{x}_t | \mathbf{y}_{1:t} \right)

smoothing

filtering

¿Estimación?

considera dos problemas

\hat{\mathbf{x}}
P\left( \mathbf{x}_{1:N},\theta | \mathbf{y}_{1:N} \right)
P\left( \mathbf{x}_t | \mathbf{y}_{1:t} \right)

smoothing

filtering

queremos que sea en tiempo real

¿Estimación?

puede ser una ventana

Filtrado bajo la propiedad de Markov

\mathbf{x}_1
\mathbf{x}_2
\mathbf{x}_{t-1}
\mathbf{x}_t
\mathbf{y}_1
\mathbf{y}_2
\mathbf{y}_{t-1}
\mathbf{y}_t

Filtrado bajo la propiedad de Markov

\mathbf{x}_1
\mathbf{x}_2
\mathbf{x}_{t-1}
\mathbf{x}_t
\mathbf{y}_1
\mathbf{y}_2
\mathbf{y}_{t-1}
\mathbf{y}_t

toda la información del pasado está contenida en el estado (memoria)

permite entonces mediante la regla de Bayes

Filtrado bajo la propiedad de Markov

\mathbf{x}_1
\mathbf{x}_2
\mathbf{x}_{t-1}
\mathbf{x}_t
\mathbf{y}_1
\mathbf{y}_2
\mathbf{y}_{t-1}
\mathbf{y}_t

toda la información del pasado está contenida en el estado (memoria)

P\left(\mathbf{x}_t | \mathbf{y}_{1:t} \right) \propto P\left(\mathbf{y}_t | \mathbf{x}_t \right) P\left(\mathbf{x}_t | \mathbf{y}_{1:t-1} \right)

permite entonces mediante la regla de Bayes

Filtrado bajo la propiedad de Markov

\mathbf{x}_1
\mathbf{x}_2
\mathbf{x}_{t-1}
\mathbf{x}_t
\mathbf{y}_1
\mathbf{y}_2
\mathbf{y}_{t-1}
\mathbf{y}_t

toda la información del pasado está contenida en el estado (memoria)

P\left(\mathbf{x}_t | \mathbf{y}_{1:t} \right) \propto P\left(\mathbf{y}_t | \mathbf{x}_t \right) P\left(\mathbf{x}_t | \mathbf{y}_{1:t-1} \right)

modelo de medición

permite entonces mediante la regla de Bayes

Filtrado bajo la propiedad de Markov

\mathbf{x}_1
\mathbf{x}_2
\mathbf{x}_{t-1}
\mathbf{x}_t
\mathbf{y}_1
\mathbf{y}_2
\mathbf{y}_{t-1}
\mathbf{y}_t

toda la información del pasado está contenida en el estado (memoria)

P\left(\mathbf{x}_t | \mathbf{y}_{1:t-1} \right) = \displaystyle \int P\left(\mathbf{x}_t | \mathbf{x}_{t-1} \right) P\left(\mathbf{x}_{t-1} | \mathbf{y}_{1:t-1} \right) d\mathbf{x}_{t-1}

permite entonces mediante la regla de Bayes

Filtrado bajo la propiedad de Markov

\mathbf{x}_1
\mathbf{x}_2
\mathbf{x}_{t-1}
\mathbf{x}_t
\mathbf{y}_1
\mathbf{y}_2
\mathbf{y}_{t-1}
\mathbf{y}_t

toda la información del pasado está contenida en el estado (memoria)

P\left(\mathbf{x}_t | \mathbf{y}_{1:t-1} \right) = \displaystyle \int P\left(\mathbf{x}_t | \mathbf{x}_{t-1} \right) P\left(\mathbf{x}_{t-1} | \mathbf{y}_{1:t-1} \right) d\mathbf{x}_{t-1}

dinámica o modelo de proceso

Entonces, el filtro de Kalman puede interpretarse tanto como un observador de estado como un filtro Bayesiano

Entonces, el filtro de Kalman puede interpretarse tanto como un observador de estado como un filtro Bayesiano

 

veamos los detalles...

Una situación familiar...

Una situación familiar...

\dot{\mathbf{x}}=\mathbf{A}\mathbf{x}+\mathbf{B}\mathbf{u}+\mathbf{F}\mathbf{w} \\ \mathbf{y}=\mathbf{C}\mathbf{x}+\mathbf{v}

...pero en tiempo discreto

\mathbf{x}_{k+1}=\mathbf{A}[k]\mathbf{x}_k+\mathbf{B}[k]\mathbf{u}_k+\mathbf{F}[k]\mathbf{w}_k \\ \mathbf{y}_k=\mathbf{C}[k]\mathbf{x}_k+\mathbf{v}_k

permite matrices variantes en el tiempo

...pero en tiempo discreto

ruido de proceso

vector de ruido blanco

ruido de medición

vector de ruido blanco

\mathbf{x}_{k+1}=\mathbf{A}[k]\mathbf{x}_k+\mathbf{B}[k]\mathbf{u}_k+\mathbf{F}[k]\mathbf{w}_k \\ \mathbf{y}_k=\mathbf{C}[k]\mathbf{x}_k+\mathbf{v}_k

...pero en tiempo discreto

\mathbf{w}_k \sim \mathcal{N}\left(\mathbf{0}, \mathbf{Q}_w[k]\right)
\mathbf{v}_k \sim \mathcal{N}\left(\mathbf{0}, \mathbf{Q}_v[k]\right)

aunque se permite que cambie la varianza en el tiempo, no es lo típico

\mathbf{x}_{k+1}=\mathbf{A}[k]\mathbf{x}_k+\mathbf{B}[k]\mathbf{u}_k+\mathbf{F}[k]\mathbf{w}_k \\ \mathbf{y}_k=\mathbf{C}[k]\mathbf{x}_k+\mathbf{v}_k

...pero en tiempo discreto

\mathbf{w}_k \sim \mathcal{N}\left(\mathbf{0}, \mathbf{Q}_w[k]\right)
\mathbf{v}_k \sim \mathcal{N}\left(\mathbf{0}, \mathbf{Q}_v[k]\right)

aunque se permite que cambie la varianza en el tiempo, no es lo típico

\mathbf{x}_{k+1}=\mathbf{A}[k]\mathbf{x}_k+\mathbf{B}[k]\mathbf{u}_k+\mathbf{F}[k]\mathbf{w}_k \\ \mathbf{y}_k=\mathbf{C}[k]\mathbf{x}_k+\mathbf{v}_k

¿Por qué tiempo discreto?

Porque el filtro puede separarse en etapas: predicción + corrección

el filtro devuelve

\hat{\mathbf{x}}_k \approx \mathbf{x}_k

el filtro devuelve

\hat{\mathbf{x}}_k \approx \mathbf{x}_k
E\left\{\hat{\mathbf{x}}_k\right\}= \mathbf{x}_k
\hat{\mathbf{x}}_k \sim \mathcal{N}\left( \mathbf{x}_k, \mathbf{P}_k \right)
\mathbf{P}_k=E\left\{\left(\hat{\mathbf{x}}_k-\mathbf{x}_k\right) \left(\hat{\mathbf{x}}_k-\mathbf{x}_k\right)^\top\right\}

encontrando la ganancia de Kalman \(\mathbf{L}_k\) que minimiza la varianza \(\mathbf{P}_k\) del error de estimación

\(\mathbf{e}_k=\hat{\mathbf{x}}-\mathbf{x}_k\)

se separa a \(\hat{\mathbf{x}}_k\) en dos (igual que a \(\mathbf{P}_k\)):

\(\hat{\mathbf{x}}_{k|k-1}\) a-priori (sin mediciones)

\(\hat{\mathbf{x}}_{k|k}\) a-posteriori (con mediciones)

condición inicial (si se conoce) o un estimado de la misma

certeza con la que se conoce la condición inicial

\(\mathbf{P}_{0|0}=E\left\{\mathbf{x}_0\mathbf{x}_0^\top\right\}=\sigma_e^2\mathbf{I}\)

innovación

\(\mathbf{z}_k=\mathbf{y}_k-\mathbf{C}[k]\hat{\mathbf{x}}_{k|k-1}\)

covarianza de la innovación

\(\mathbf{S}[k]=\mathbf{Q}_v[k]+\mathbf{C}[k]\mathbf{P}_{k|k-1}\mathbf{C}[k]^\top\)

intuición: >> mt3006_clase11_kalman1d.m

ejemplo: >> mt3006_clase11_kalman_fusion.mlx

El filtro de Kalman Extendido (EKF)

extiende el filtro al caso no lineal

\mathbf{x}_{k+1}=\boldsymbol{\mathcal{F}}\left(\mathbf{x}_k,\mathbf{u}_k,\mathbf{w}_k\right) \newline \mathbf{y}_k=\boldsymbol{\mathcal{H}}\left(\mathbf{x}_k\right)+\mathbf{v}_k

redefiniendo la predicción y corrección de estado y generando las matrices \(\mathbf{A}, \mathbf{B}, \mathbf{F}, \mathbf{C}\) mediante linealización (jacobianos)

\mathbf{A}[k]=\dfrac{ \boldsymbol{\mathcal{F}}\left( \hat{\mathbf{x}}_{k-1 | k-1}, \mathbf{u}_{k-1}, \mathbf{0} \right)} {\mathbf{x}_k}
\mathbf{B}[k]=\dfrac{ \boldsymbol{\mathcal{F}}\left( \hat{\mathbf{x}}_{k-1 | k-1}, \mathbf{u}_{k-1}, \mathbf{0} \right)} {\mathbf{u}_k}
\mathbf{F}[k]=\dfrac{ \boldsymbol{\mathcal{F}}\left( \hat{\mathbf{x}}_{k-1 | k-1}, \mathbf{u}_{k-1}, \mathbf{0} \right)} {\mathbf{w}_k}
\mathbf{C}[k]=\dfrac{ \boldsymbol{\mathcal{H}}\left( \hat{\mathbf{x}}_{k | k-1} \right)} {\mathbf{x}_k}
\hat{\mathbf{x}}_{k|k-1}=\boldsymbol{\mathcal{F}}\left(\hat{\mathbf{x}}_{k-1|k-1},\mathbf{u}_{k-1},\mathbf{0}\right)
\hat{\mathbf{x}}_{k|k}=\hat{\mathbf{x}}_{k|k-1}+\mathbf{L}_k\left(\mathbf{y}_k-\boldsymbol{\mathcal{H}}\left(\hat{\mathbf{x}}_{k|k-1}\right)\right)

Regresando a fusión de sensores

Predicción

Corrección

medición

sensores propioceptivos

sensores exteroceptivos

filtro de Kalman

Predicción

Corrección

medición

sensores propioceptivos

sensores exteroceptivos

filtro de Kalman

ambos son modelos de medición (sensores)*

ambos son modelos de medición (sensores)*

Predicción

Corrección

medición

sensores propioceptivos

sensores exteroceptivos

filtro de Kalman

* se colocan los propioceptivos en la predicción dado que estos típicamente miden cantidades diferenciales | incrementales, por lo mismo presentan "dinámica".

supongamos que quiere encontrarse cierta cantidad \(z\) y la misma se mide con dos sensores

Ejemplo

Referencias

  1. MT3006 - Localización y mapeo en robótica móvil.pdf
  2. Kok et. al, Using Inertial Sensors for Position and Orientation Estimation.

Predicción

Corrección

medición

sensores propioceptivos

sensores exteroceptivos

filtro de Kalman

IMPORTANTE: diferenciar el uso del filtro como filtro vs como observador