El filtro de Kalman: predecir, corregir y repetir · Estimación y filtro de Kalman: seguir un objeto con medidas ruidosas · CO.DE Academy
Lección 4 de 9· Control y estimación
El filtro de Kalman: predecir, corregir y repetir
0 de 9 lecciones · 0 %
01 / 03
En los videos
4.1 Predecir: la estimación avanza con el modelo, la incertidumbre se hincha con ΦPΦ⊤+Q y, sin medidas, la σ de posición pasa de 2.24 km a 20.43 km en veinte pasos.
4.2 Corregir: la innovación, su covarianza S, la ganancia K=(0.33,0.08) y la elipse que se encoge al llegar la medida.
4.3 El ciclo completo: cinco ecuaciones en lazo, una sonda seguida en el plano y cuarenta pasos en los que el filtro estima incluso la velocidad que nadie mide.
El problema que resolvemos
Una sonda avanza por una trayectoria casi recta. Cada segundo, una estación da su posición con un error típico de 2 km (R=4 km²). El modelo dice que la velocidad es casi constante, salvo pequeñas aceleraciones de densidad q=0.005. Queremos, en cada instante, la mejor estimación de posición y velocidad, y saber cuánto podemos fiarnos de ella. Las lecciones anteriores dejaron las piezas: un modelo de estado con Φ y Q (lección 2), y las reglas para propagar y fusionar gaussianas (lección 3). El filtro de Kalman es encadenarlas.
Es el modelo del curso, el mismo de las lecciones 4 a 8:
Si en el paso k−1 la estimación es x^k−1 con covarianza Pk−1, el modelo dice que el estado avanza con Φ y recibe un ruido w de media cero. Tomando esperanzas y covarianzas, exactamente como en la lección 3:
xk−=Φx^k−1,Pk−=ΦPk−1Φ⊤+Q.
Ejemplo resuelto. De x^=(10 km,0.5 km/s) la predicción es (10.5,0.5): la posición suma la velocidad y la velocidad no cambia. Con P=diag(1,0.25), la cizalla da P11=1.25 y Q añade 0.005/3=0.0017: P11−=1.2517 y σ pasa de 1 a 1.12 km.
Sin medidas, la predicción se repite y la incertidumbre solo crece. Desde P0=diag(4,1), σ vale 2.24 km tras un paso (4+1+0.0017), 10.28 km tras diez y 20.43 km tras veinte. Es lo que se pierde durante un apagón de comunicaciones: la elipse se estira 4.6 veces en posición, aunque su área apenas crece (la cizalla la conserva y Q es pequeño). Lo que crece es la duda sobre dónde está la sonda.
Mitad dos: corregir
Llega una medida zk. Antes de usarla, el filtro calcula lo que esperaba medir, Hxk−, y la sorpresa:
yk=zk−Hxk−,Sk=HPk−H⊤+R.
y es la innovación y S su covarianza prevista: la incertidumbre de la predicción vista por el sensor más la del propio sensor.
Deducción de la ganancia. Buscamos una corrección lineal x^=x−+Ky que minimice el error cuadrático medio, es decir, la traza de la covarianza posterior. Con e−=x−x− y y=He−+v, el error posterior es e=(I−KH)e−−Kv, y como e− y v son independientes,
P=(I−KH)P−(I−KH)⊤+KRK⊤=P−−KHP−−P−H⊤K⊤+KSK⊤.
Derivando la traza respecto de K (usando ∂tr(KA)/∂K=A⊤ y ∂tr(KSK⊤)/∂K=2KS):
∂K∂trP=−2P−H⊤+2KS=0⟹K=P−H⊤S−1.
Sustituyendo esa K, los dos términos cruzados y KSK⊤ se simplifican y queda P=(I−KH)P−. La primera forma, la de Joseph, es la que usan los laboratorios: vale para cualquier K y mantiene P simétrica frente al redondeo.
Ejemplo resuelto: una corrección del video 4.2
Predicción x−=(10.50,0.50), P−=[20.50.50.4], medida z=12 km, R=4.
Covarianza: P11=(1−0.333)⋅2=1.333, σ de 1.41 a 1.15 km.
El área de la elipse se multiplica por detP/detP−=R/S=4/6=0.816, el 82 % del video. Con un sensor excelente (R=0.25) quedaría en un tercio; con uno malo (R=100), en el 99 %. Una medida solo puede restar incertidumbre.
El ciclo completo
Cinco ecuaciones, dos para predecir y tres para corregir:
En el primer paso del modelo del curso, P11−=5.0017, S=9.0017 y K=(0.556,0.111); la σ baja de 2.24 a 1.49 km. En el quinto, K1=0.46; en el cuadragésimo, 0.23. El filtro se fía cada vez menos de cada medida nueva, porque su predicción ya es buena.
Lo más llamativo es la velocidad. El sensor no la mide. Derivarla restando medidas consecutivas suma el ruido de dos medidas: σ2/T=2.83 km/s. El filtro, en cambio, la estima con un error del orden de 0.3 km/s en la simulación del video (2.75 frente a 0.29 km/s). La razón está en la segunda componente de K, que estudiaremos en la lección 5.
Playground · python
El programa repite los ejemplos y corre los cuarenta pasos con el generador de Python (semilla 46). En esa realización imprime 1.77 km de error RMS para las medidas y 1.06 km para el filtro, y 2.32 frente a 0.26 km/s en velocidad: cifras distintas de las del video, que usa otro generador, pero con la misma historia.
El laboratorio
En Ciclo predecir-corregir avanzas el filtro media vuelta cada vez. El plano posición-velocidad muestra la elipse violeta de la predicción, la banda ámbar de la medida y la elipse verde corregida. Puedes arrastrar la medida: la estimación se mueve K1 veces lo que la muevas en posición y K2 en velocidad, pero la elipse verde no cambia, porque P no depende de z. Debajo, la sonda en el tiempo y la ganancia paso a paso.
1 de 6
En el paso 1 del modelo del curso, P11−=5.0017 y R=4. ¿Qué fracción de la innovación se suma a la posición?
Qué deja fuera este modelo
Optimalidad condicionada. El filtro es el estimador de mínima varianza si el modelo es lineal, los ruidos son blancos, gaussianos e independientes y Q, R son los verdaderos. Si no, sigue siendo el mejor estimador lineal con esas Q y R, pero ya no el óptimo.
Velocidad constante. Una maniobra o una aceleración sostenida no están en el modelo; el filtro las verá como innovaciones grandes y llegará tarde (lección 6).
Medidas perfectamente sincronizadas. Aquí llega una medida por paso; los sensores reales llegan a ritmos distintos, tarde o nunca (lección 8).
Aritmética exacta. Con números en coma flotante, P=(I−KH)P− puede perder simetría o positividad en filtros grandes; por eso existen las formas de Joseph y las factorizadas.
Lo que sigue
La ganancia es una columna de dos números y la segunda es la que encuentra la velocidad. En la lección 5 abrimos K: qué parte de la innovación va a cada estado, qué estados se pueden estimar (observabilidad) y por qué K converge a un valor fijo, el de la ecuación de Riccati.
Laboratorio interactivo
El ciclo predecir-corregir paso a paso
Cierre de la lección
Lo que sigue: video
Sigue el recorrido de la lección paso a paso.
Crea una cuenta gratis para continuar
Con una cuenta desbloqueas el cuestionario, el foro de dudas y el tutor con IA de esta lección, además del registro de progreso y el certificado verificable al terminar «Estimación y filtro de Kalman: seguir un objeto con medidas ruidosas».