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
B
\mathbf{B}
+
\boldsymbol{+}
∫
\boldsymbol{\int}
C
\mathbf{C}
A
\mathbf{A}
u
\mathbf{u}
y
\mathbf{y}
x˙
\dot{\mathbf{x}}
x
\mathbf{x}
K
\mathbf{K}
x
\mathbf{x}
(−1)
(-1)
controlador lineal
state feedback
retroalimentación negativa
???
\boldsymbol{???}
observador de estado
x^≈x
\hat{\mathbf{x}} \approx \mathbf{x}
¿Qué es?
¿Cuándo funciona?
(valor) estimado del estado
Un flashback a control moderno
y
\mathbf{y}
x^≈x
\hat{\mathbf{x}} \approx \mathbf{x}
es un estimador
???
???
sistema que genera estimados | estimaciones
≜
\triangleq
Según Luenberger
x^˙=Ax^+Bu+L(y−y^)
\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
x^˙=Ax^+Bu+L(y−y^)
\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 ∈Rn×p
mediciones
salida según el modelo LTI y^=Cx^
Pero existe otra manera de interpretar el problema, desde una perspectiva de estimación
¿Estimación?
considera dos problemas
P(x1:N,θ∣y1:N)
P\left( \mathbf{x}_{1:N},\theta | \mathbf{y}_{1:N} \right)
P(xt∣y1:t)
P\left( \mathbf{x}_t | \mathbf{y}_{1:t} \right)
considera dos problemas
x^
\hat{\mathbf{x}}
P(x1:N,θ∣y1:N)
P\left( \mathbf{x}_{1:N},\theta | \mathbf{y}_{1:N} \right)
P(xt∣y1:t)
P\left( \mathbf{x}_t | \mathbf{y}_{1:t} \right)
smoothing
¿Estimación?
considera dos problemas
x^
\hat{\mathbf{x}}
P(x1:N,θ∣y1:N)
P\left( \mathbf{x}_{1:N},\theta | \mathbf{y}_{1:N} \right)
P(xt∣y1:t)
P\left( \mathbf{x}_t | \mathbf{y}_{1:t} \right)
smoothing
filtering
¿Estimación?
considera dos problemas
x^
\hat{\mathbf{x}}
P(x1:N,θ∣y1:N)
P\left( \mathbf{x}_{1:N},\theta | \mathbf{y}_{1:N} \right)
P(xt∣y1:t)
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
x1
\mathbf{x}_1
x2
\mathbf{x}_2
xt−1
\mathbf{x}_{t-1}
xt
\mathbf{x}_t
y1
\mathbf{y}_1
y2
\mathbf{y}_2
yt−1
\mathbf{y}_{t-1}
yt
\mathbf{y}_t
Filtrado bajo la propiedad de Markov
x1
\mathbf{x}_1
x2
\mathbf{x}_2
xt−1
\mathbf{x}_{t-1}
xt
\mathbf{x}_t
y1
\mathbf{y}_1
y2
\mathbf{y}_2
yt−1
\mathbf{y}_{t-1}
yt
\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
x1
\mathbf{x}_1
x2
\mathbf{x}_2
xt−1
\mathbf{x}_{t-1}
xt
\mathbf{x}_t
y1
\mathbf{y}_1
y2
\mathbf{y}_2
yt−1
\mathbf{y}_{t-1}
yt
\mathbf{y}_t
toda la información del pasado está contenida en el estado (memoria)
P(xt∣y1:t)∝P(yt∣xt)P(xt∣y1:t−1)
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
x1
\mathbf{x}_1
x2
\mathbf{x}_2
xt−1
\mathbf{x}_{t-1}
xt
\mathbf{x}_t
y1
\mathbf{y}_1
y2
\mathbf{y}_2
yt−1
\mathbf{y}_{t-1}
yt
\mathbf{y}_t
toda la información del pasado está contenida en el estado (memoria)
P(xt∣y1:t)∝P(yt∣xt)P(xt∣y1:t−1)
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
x1
\mathbf{x}_1
x2
\mathbf{x}_2
xt−1
\mathbf{x}_{t-1}
xt
\mathbf{x}_t
y1
\mathbf{y}_1
y2
\mathbf{y}_2
yt−1
\mathbf{y}_{t-1}
yt
\mathbf{y}_t
toda la información del pasado está contenida en el estado (memoria)
P(xt∣y1:t−1)=∫P(xt∣xt−1)P(xt−1∣y1:t−1)dxt−1
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
x1
\mathbf{x}_1
x2
\mathbf{x}_2
xt−1
\mathbf{x}_{t-1}
xt
\mathbf{x}_t
y1
\mathbf{y}_1
y2
\mathbf{y}_2
yt−1
\mathbf{y}_{t-1}
yt
\mathbf{y}_t
toda la información del pasado está contenida en el estado (memoria)
P(xt∣y1:t−1)=∫P(xt∣xt−1)P(xt−1∣y1:t−1)dxt−1
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...
x˙=Ax+Bu+Fwy=Cx+v
\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
xk+1=A[k]xk+B[k]uk+F[k]wkyk=C[k]xk+vk
\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
xk+1=A[k]xk+B[k]uk+F[k]wkyk=C[k]xk+vk
\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
wk∼N(0,Qw[k])
\mathbf{w}_k \sim \mathcal{N}\left(\mathbf{0}, \mathbf{Q}_w[k]\right)
vk∼N(0,Qv[k])
\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
xk+1=A[k]xk+B[k]uk+F[k]wkyk=C[k]xk+vk
\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
wk∼N(0,Qw[k])
\mathbf{w}_k \sim \mathcal{N}\left(\mathbf{0}, \mathbf{Q}_w[k]\right)
vk∼N(0,Qv[k])
\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
xk+1=A[k]xk+B[k]uk+F[k]wkyk=C[k]xk+vk
\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
x^k≈xk
\hat{\mathbf{x}}_k \approx \mathbf{x}_k
el filtro devuelve
x^k≈xk
\hat{\mathbf{x}}_k \approx \mathbf{x}_k
E{x^k}=xk
E\left\{\hat{\mathbf{x}}_k\right\}= \mathbf{x}_k
x^k∼N(xk,Pk)
\hat{\mathbf{x}}_k \sim \mathcal{N}\left( \mathbf{x}_k, \mathbf{P}_k \right)
Pk=E{(x^k−xk)(x^k−xk)⊤}
\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 Lk que minimiza la varianza Pk del error de estimación
ek=x^−xk
se separa a x^k en dos (igual que a Pk):
x^k∣k−1 a-priori (sin mediciones)
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
P0∣0=E{x0x0⊤}=σe2I
innovación
zk=yk−C[k]x^k∣k−1
covarianza de la innovación
S[k]=Qv[k]+C[k]Pk∣k−1C[k]⊤
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
xk+1=F(xk,uk,wk)yk=H(xk)+vk
\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 A,B,F,C mediante linealización (jacobianos)
A[k]=xkF(x^k−1∣k−1,uk−1,0)
\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}
B[k]=ukF(x^k−1∣k−1,uk−1,0)
\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}
F[k]=wkF(x^k−1∣k−1,uk−1,0)
\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}
C[k]=xkH(x^k∣k−1)
\mathbf{C}[k]=\dfrac{ \boldsymbol{\mathcal{H}}\left( \hat{\mathbf{x}}_{k | k-1} \right)} {\mathbf{x}_k}
x^k∣k−1=F(x^k−1∣k−1,uk−1,0)
\hat{\mathbf{x}}_{k|k-1}=\boldsymbol{\mathcal{F}}\left(\hat{\mathbf{x}}_{k-1|k-1},\mathbf{u}_{k-1},\mathbf{0}\right)
x^k∣k=x^k∣k−1+Lk(yk−H(x^k∣k−1))
\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".
Referencias
- MT3006 - Localización y mapeo en robótica móvil.pdf
- Kok et. al, Using Inertial Sensors for Position and Orientation Estimation.
El filtro de Kalman y sus variantes MT3006 - Robótica 2
MT3006 - Lecture 11 (2024)
By Miguel Enrique Zea Arenales
MT3006 - Lecture 11 (2024)
- 211
