El algoritmo de filtro de Kalman funciona de la siguiente manera
Inicializar x 0 | 0 y P 0 | 0 .
En cada iteración
Predecir
Predicho (a priori) del estado estimación x k | k - 1 = F k x k - 1 | k - 1 + B k u k Estimación de covarianza estimada (a priori) P k | k - 1 = F k P k - 1 | k - 1 F T k + Q k Actualizar
Innovación o medición residual Covarianza de innovación (o residual) S k = H k P k | k - 1 H T k + R k Ganancia óptima de Kalman K k = P k | k - 1 H T k S - 1 k
Actualizado (a posteriori) Estado estimación x k | k = x k | k - 1 + K k ˜ y k Actualización (a posteriori) de covarianza estimada P k | k = ( I - K k H k ) P k | k - 1
¡Gracias y saludos!