Estimación Robusta de Estados con Filtros de Kalman Extendidos e Inodoros para Machine Learning

🚀 Introducción a la Estimación de Estados en Sistemas No Lineales

En el fascinante mundo del Machine Learning y la robótica, a menudo nos enfrentamos al desafío de estimar el estado interno de un sistema a partir de mediciones ruidosas y un modelo dinámico. Cuando el sistema es lineal, el venerable Filtro de Kalman es la herramienta ideal. Sin embargo, la realidad de muchos problemas, desde el seguimiento de objetos hasta la fusión de sensores en vehículos autónomos, implica dinámicas no lineales. Aquí es donde los Filtros de Kalman Extendidos (EKF) y los Filtros de Kalman Inodoros (UKF) entran en juego, ofreciendo soluciones para navegar por esta complejidad.

Este tutorial te guiará a través de los conceptos fundamentales, la implementación práctica y una comparación crucial entre EKF y UKF, equipándote con el conocimiento para aplicar estas poderosas técnicas en tus propios proyectos.

¿Por qué necesitamos técnicas robustas de estimación?

Imagina un robot móvil tratando de determinar su posición exacta utilizando sensores que introducen errores, o un modelo financiero que predice el valor de una acción basándose en indicadores imperfectos. En ambos casos, las mediciones están contaminadas con ruido y el modelo que describe la evolución del sistema puede ser una aproximación. La estimación de estados busca encontrar la mejor predicción del estado real del sistema, minimizando la incertidumbre.

📖 Repaso del Filtro de Kalman Clásico (Lineal)

Antes de sumergirnos en los filtros no lineales, es útil recordar brevemente cómo funciona el Filtro de Kalman lineal. Es un algoritmo recursivo que estima el estado de un sistema dinámico lineal a partir de una serie de mediciones ruidosas. Opera en dos fases principales:

1. Predicción: Estima el estado actual y su covarianza basándose en el estado anterior y el modelo de movimiento del sistema.
2. Actualización: Corrige la estimación predicha utilizando la medición actual y su covarianza, ponderando la confianza entre la predicción y la medición.

Fórmulas Clave (Lineal)

• Estado: x
• Covarianza: P
• Matriz de transición de estado: A
• Matriz de control: B
• Vector de control: u
• Matriz de observación: H
• Covarianza del ruido de proceso: Q
• Covarianza del ruido de medición: R

Fase de Predicción:

• Predicción del estado: x_hat_minus = A * x_hat_plus + B * u
• Predicción de la covarianza: P_minus = A * P_plus * A.T + Q

Fase de Actualización:

• Residual de medición: y = z - H * x_hat_minus
• Covarianza residual: S = H * P_minus * H.T + R
• Ganancia de Kalman: K = P_minus * H.T * S.inv
• Actualización del estado: x_hat_plus = x_hat_minus + K * y
• Actualización de la covarianza: P_plus = (I - K * H) * P_minus

Esta estructura recursiva es la base sobre la que se construyen los filtros no lineales.

🚧 Filtro de Kalman Extendido (EKF): El Enfoque de Linealización

El EKF es la extensión más intuitiva del Filtro de Kalman para sistemas no lineales. La idea central es linealizar las funciones no lineales alrededor del punto de la estimación actual utilizando aproximaciones de primer orden de las series de Taylor. Esto significa que las matrices A y H del filtro lineal son reemplazadas por las matrices Jacobianas de las funciones de transición de estado (f) y de medición (h).

📝 Funcionamiento del EKF

1. Modelo del Sistema:

   • Transición de estado: x_k = f(x_{k-1}, u_k) + w_k
   • Medición: z_k = h(x_k) + v_k Donde w_k y v_k son ruidos de proceso y medición, respectivamente, con covarianza Q y R.

2. Fase de Predicción:

   • Se predice el estado x_k_minus aplicando la función no lineal f al estado anterior x_{k-1}.
   • Se calcula la matriz Jacobiana F_k de f con respecto a x en x_{k-1}.
   • La covarianza P_k_minus se predice usando F_k.

   x_k_minus = f(x_{k-1}, u_k) F_k = jacobian(f, x_{k-1}) P_k_minus = F_k * P_{k-1} * F_k.T + Q

3. Fase de Actualización:

   • Se predice la medición h_k_minus aplicando la función no lineal h al estado predicho x_k_minus.
   • Se calcula la matriz Jacobiana H_k de h con respecto a x en x_k_minus.
   • Las ecuaciones restantes son idénticas al Filtro de Kalman lineal, pero utilizando H_k.

   y = z_k - h(x_k_minus) H_k = jacobian(h, x_k_minus) S = H_k * P_k_minus * H_k.T + R K = P_k_minus * H_k.T * S.inv x_k = x_k_minus + K * y P_k = (I - K * H_k) * P_k_minus

✅ Pros y Contras del EKF

🛠️ Implementación en Python (Esqueleto)

import numpy as np
from scipy.linalg import block_diag
from scipy.optimize import approx_fprime

class EKF:
    def __init__(self, f, h, Q, R, x0, P0):
        self.f = f  # Función de transición de estado (no lineal)
        self.h = h  # Función de observación (no lineal)
        self.Q = Q  # Covarianza del ruido de proceso
        self.R = R  # Covarianza del ruido de medición
        self.x = x0 # Estado inicial
        self.P = P0 # Covarianza inicial del estado
        self.
        self.I = np.eye(x0.shape[0])

    def predict(self, u=None):
        # Predicción del estado
        if u is None:
            self.x = self.f(self.x) # Asume f(x) si no hay control
        else:
            self.x = self.f(self.x, u) # Asume f(x, u)

        # Calcular Jacobiano F de f en self.x
        F = self._jacobian(self.f, self.x, u_args=u) # Método auxiliar para Jacobiano

        # Predicción de la covarianza
        self.P = F @ self.P @ F.T + self.Q

        return self.x

    def update(self, z):
        # Predicción de la medición
        hx = self.h(self.x)

        # Calcular Jacobiano H de h en self.x
        H = self._jacobian(self.h, self.x) # Método auxiliar para Jacobiano

        # Residual de medición
        y = z - hx

        # Covarianza residual
        S = H @ self.P @ H.T + self.R

        # Ganancia de Kalman
        K = self.P @ H.T @ np.linalg.inv(S)

        # Actualización del estado
        self.x = self.x + K @ y

        # Actualización de la covarianza
        self.P = (self.I - K @ H) @ self.P

        return self.x, self.P

    def _jacobian(self, func, x, u_args=None, epsilon=1e-6):
        # Aproximación numérica del Jacobiano
        # Esto es una simplificación; en producción, se preferiría el Jacobiano analítico si es posible.
        # Para funciones f(x,u) el Jacobiano es solo respecto a x
        n = len(x)
        jacobian = np.zeros((len(func(x, u_args) if u_args is not None else func(x)), n))

        for i in range(n):
            x_plus = x.copy()
            x_minus = x.copy()
            x_plus[i] += epsilon
            x_minus[i] -= epsilon

            f_plus = func(x_plus, u_args) if u_args is not None else func(x_plus)
            f_minus = func(x_minus, u_args) if u_args is not None else func(x_minus)

            jacobian[:, i] = (f_plus - f_minus) / (2 * epsilon)
        return jacobian

✨ Filtro de Kalman Inodoro (UKF): Propagación de Puntos Sigma

El UKF aborda el problema de la linealización explícita del EKF de una manera diferente y, a menudo, más robusta. En lugar de linealizar las funciones no lineales, el UKF utiliza una técnica llamada transformación inodora (Unscented Transform). Esta técnica propaga un conjunto de puntos de muestra estratégicamente elegidos (llamados puntos sigma) a través de las funciones no lineales. La media y la covarianza del estado se recuperan de los puntos transformados.

📝 Funcionamiento del UKF

El UKF también consta de fases de predicción y actualización, pero la forma en que manejan las no linealidades es clave.

1. Generación de Puntos Sigma:
   • Dado un estado x y su covarianza P, se generan 2n + 1 puntos sigma (donde n es la dimensión del estado). Estos puntos están diseñados para capturar la media y la covarianza de la distribución del estado.

2. Fase de Predicción:

   • Cada punto sigma se propaga a través de la función no lineal de transición de estado f.
   • Se calculan las nuevas medias y covarianzas predichas de estos puntos sigma transformados, incorporando el ruido de proceso Q.

3. Fase de Actualización:

   • Los puntos sigma predichos se usan para generar un nuevo conjunto de puntos sigma para la fase de medición.
   • Cada uno de estos puntos sigma se propaga a través de la función no lineal de observación h.
   • Se calcula la media y la covarianza de las mediciones predichas.
   • Se calcula la covarianza cruzada entre el estado y la medición.
   • Se calcula la ganancia de Kalman y se actualizan el estado y la covarianza, de manera similar al EKF.

✅ Pros y Contras del UKF

🛠️ Implementación en Python (Esqueleto)

La implementación del UKF es más compleja que el EKF debido a la manipulación de los puntos sigma y los pesos asociados. Aquí un esqueleto que muestra la estructura básica.

import numpy as np
from filterpy.kalman import UnscentedKalmanFilter, MerweScaledSigmaPoints

class UKF:
    def __init__(self, dim_x, dim_z, dt, hx, fx, Q, R, x0, P0):
        # Definir los puntos sigma (MerweScaledSigmaPoints es una buena opción por defecto)
        points = MerweScaledSigmaPoints(n=dim_x, alpha=0.01, beta=2., kappa=0.)

        self.ukf = UnscentedKalmanFilter(
            dim_x=dim_x, 
            dim_z=dim_z, 
            dt=dt, 
            hx=hx, 
            fx=fx, 
            points=points
        )

        self.ukf.x = x0  # Estado inicial
        self.ukf.P = P0  # Covarianza inicial del estado
        self.ukf.Q = Q   # Covarianza del ruido de proceso
        self.ukf.R = R   # Covarianza del ruido de medición

    def predict(self):
        self.ukf.predict()
        return self.ukf.x

    def update(self, z):
        self.ukf.update(z)
        return self.ukf.x, self.ukf.P

# Ejemplo de uso (necesita funciones hx y fx definidas)
# def fx_radar(x, dt):
#     # Modelo de movimiento no lineal para un objeto (e.g., radar)
#     # x = [pos_x, pos_y, vel_x, vel_y, acc_x, acc_y]
#     # Implementar la ecuación de movimiento
#     ...
#     return new_x

# def hx_radar(x):
#     # Modelo de observación no lineal (e.g., distancia y ángulo desde un radar)
#     # x = [pos_x, pos_y, ...]
#     # return [distancia, angulo]
#     ...
#     return h_x

# dim_x = 6 # e.g., [x, y, vx, vy, ax, ay]
# dim_z = 2 # e.g., [range, bearing]
# dt = 0.1
# Q = np.diag([0.1, 0.1, 0.1, 0.1, 0.1, 0.1])**2
# R = np.diag([0.1, np.deg2rad(1)])**2 # Error de distancia y ángulo
# x0 = np.array([0., 0., 0., 0., 0., 0.])
# P0 = np.eye(dim_x) * 0.1

# ukf_filter = UKF(dim_x, dim_z, dt, hx_radar, fx_radar, Q, R, x0, P0)

📊 Comparación EKF vs. UKF en Machine Learning

La elección entre EKF y UKF depende en gran medida de las características específicas del problema:

Escenarios de Aplicación Típicos:

• EKF: Ideal para sistemas donde las funciones no lineales son relativamente suaves, o cuando el estado de error permanece pequeño. Común en sistemas de navegación inercial (INS) con pequeñas perturbaciones.
• UKF: Preferible en aplicaciones donde las no linealidades son significativas o cuando se requiere una mayor precisión en la estimación de la covarianza. Ejemplos incluyen seguimiento de blancos con radares o sonares, robótica con dinámicas complejas o estimación de parámetros en modelos de Machine Learning no lineales.

Ejemplo: Seguimiento de Objetos con Modelo No Lineal

Consideremos el seguimiento de un objeto en 2D que se mueve con una velocidad constante, pero cuyas mediciones provienen de un sensor que mide la distancia y el ángulo (coordenadas polares), que son funciones no lineales de las coordenadas cartesianas del objeto.

Estado: x = [pos_x, pos_y, vel_x, vel_y]

Función de transición de estado f(x) (lineal para este ejemplo):

# x_k = F @ x_{k-1}
# F es una matriz de transición de estado lineal si asumimos velocidad constante

Función de observación h(x) (no lineal):

# z_k = [distancia, angulo]
# distancia = sqrt(x_pos^2 + y_pos^2)
# angulo = atan2(y_pos, x_pos)

def h_nonlinear(x):
    px = x[0]
    py = x[1]
    dist = np.sqrt(px**2 + py**2)
    angle = np.arctan2(py, px)
    return np.array([dist, angle])

Este es un caso de uso perfecto para EKF o UKF, ya que la dinámica del sistema puede ser lineal, pero la observación es no lineal. Aquí, el UKF generalmente superará al EKF en precisión, especialmente si el objeto se mueve en los límites del campo de visión o con giros bruscos, donde la linealización del EKF podría ser imprecisa.

🎯 Implementación Completa y Simulación (usando filterpy)

Para ilustrar y comparar EKF y UKF, utilizaremos la librería filterpy, que proporciona implementaciones robustas de ambos. Esto nos permitirá centrarnos en la aplicación y los resultados.

Configuración del Entorno

Necesitarás numpy y filterpy.

pip install numpy filterpy

Ejemplo de Simulación: Radar de Seguimiento

Vamos a simular un objeto moviéndose y un radar midiendo su distancia y ángulo.

Modelo de Estado: Estado x = [pos_x, pos_y, vel_x, vel_y]

Función de Transición (f_radar): Modelo de velocidad constante.

def f_radar(x, dt):
    # x = [pos_x, pos_y, vel_x, vel_y]
    F = np.array([  [1, 0, dt, 0],
                    [0, 1, 0, dt],
                    [0, 0, 1, 0],
                    [0, 0, 0, 1]])
    return F @ x

Función de Observación (h_radar): Conversión de coordenadas cartesianas a polares (distancia, ángulo).

def h_radar(x):
    px = x[0]
    py = x[1]
    dist = np.sqrt(px**2 + py**2)
    angle = np.arctan2(py, px)
    return np.array([dist, angle])

Código de Simulación y Comparación

import numpy as np
import matplotlib.pyplot as plt
from filterpy.kalman import ExtendedKalmanFilter, UnscentedKalmanFilter, MerweScaledSigmaPoints
from scipy.stats import multivariate_normal

# --- 1. Definición del Modelo y Parámetros --- 

dt = 0.1 # Intervalo de tiempo

# Funciones de transición de estado y observación
def f_radar(x, dt):
    F = np.array([  [1, 0, dt, 0],
                    [0, 1, 0, dt],
                    [0, 0, 1, 0],
                    [0, 0, 0, 1]])
    return F @ x

def H_jacobian_radar(x):
    # Jacobiano de h_radar(x) para EKF
    px, py, vx, vy = x[0], x[1], x[2], x[3]
    denom = px**2 + py**2
    dist = np.sqrt(denom)

    H = np.array([
        [px/dist, py/dist, 0, 0],
        [-py/denom, px/denom, 0, 0]
    ])
    return H

# Parámetros del filtro
dim_x = 4 # [pos_x, pos_y, vel_x, vel_y]
dim_z = 2 # [distancia, angulo]

# Covarianza inicial del estado
P0 = np.diag([100., 100., 10., 10.]) # Alta incertidumbre inicial

# Covarianza del ruido de proceso (velocidad constante con ruido)
# Q_cv = block_diag(Q_sub, Q_sub) # para dos dimensiones independientes
Q = np.diag([0.01, 0.01, 0.1, 0.1]) # Pequeño ruido de proceso

# Covarianza del ruido de medición (radar)
R = np.diag([0.5, np.deg2rad(1.)])**2 # Error de distancia (m) y ángulo (radianes)

# Estado inicial real
x_true_init = np.array([0., 0., 10., 10.]) # x=0, y=0, vx=10, vy=10

# --- 2. Generación de Datos Sintéticos --- 

num_steps = 100
time_points = np.arange(0, num_steps * dt, dt)

x_true = np.zeros((num_steps, dim_x))
z_measurements = np.zeros((num_steps, dim_z))

x_current = x_true_init.copy()

for i in range(num_steps):
    x_true[i] = x_current
    # Simular movimiento con un pequeño ruido de proceso
    process_noise = multivariate_normal.rvs(cov=Q)
    x_current = f_radar(x_current, dt) + process_noise 

    # Simular mediciones con ruido de medición
    z_measurements[i] = h_radar(x_true[i]) + multivariate_normal.rvs(cov=R)

# --- 3. Inicialización y Ejecución de EKF y UKF --- 

# EKF
ekf = ExtendedKalmanFilter(dim_x=dim_x, dim_z=dim_z)
ekf.x = x_true_init + np.random.randn(dim_x) * 5 # Estado inicial con ruido
ekf.P = P0
ekf.Q = Q
ekf.R = R
ekf.F = np.array([  [1, 0, dt, 0],
                    [0, 1, 0, dt],
                    [0, 0, 1, 0],
                    [0, 0, 0, 1]]) # Para EKF, la matriz F es lineal

ekf_states = np.zeros((num_steps, dim_x))

# UKF
points = MerweScaledSigmaPoints(n=dim_x, alpha=0.1, beta=2., kappa=0.) # Ajustar alpha, beta, kappa para rendimiento
ukf = UnscentedKalmanFilter(dim_x=dim_x, dim_z=dim_z, dt=dt, hx=h_radar, fx=f_radar, points=points)
ukf.x = x_true_init + np.random.randn(dim_x) * 5 # Estado inicial con ruido
ukf.P = P0
ukf.Q = Q
ukf.R = R

ukf_states = np.zeros((num_steps, dim_x))

# Ejecución de los filtros
for i, z in enumerate(z_measurements):
    # EKF
    ekf.predict()
    ekf.update(z, HJacobian=H_jacobian_radar, Hx=h_radar)
    ekf_states[i] = ekf.x

    # UKF
    ukf.predict()
    ukf.update(z)
    ukf_states[i] = ukf.x

# --- 4. Visualización de Resultados --- 

plt.figure(figsize=(12, 8))
plt.plot(x_true[:, 0], x_true[:, 1], 'g--', label='Trayectoria Real')
plt.plot(ekf_states[:, 0], ekf_states[:, 1], 'r-', label='Estimación EKF')
plt.plot(ukf_states[:, 0], ukf_states[:, 1], 'b-', label='Estimación UKF')
plt.scatter(z_measurements[:, 0] * np.cos(z_measurements[:, 1]), 
            z_measurements[:, 0] * np.sin(z_measurements[:, 1]), 
            c='k', marker='.', s=10, alpha=0.3, label='Mediciones (Polar a Cartesiana)')

plt.title('Comparación de Seguimiento de Objetos: EKF vs UKF')
plt.xlabel('Posición X')
plt.ylabel('Posición Y')
plt.legend()
plt.grid(True)
plt.show()

# Métricas de error (RMSE)
rmse_ekf = np.sqrt(np.mean((x_true[:, :2] - ekf_states[:, :2])**2))
rmse_ukf = np.sqrt(np.mean((x_true[:, :2] - ukf_states[:, :2])**2))

print(f"RMSE EKF (Posición): {rmse_ekf:.2f}")
print(f"RMSE UKF (Posición): {rmse_ukf:.2f}")

El gráfico mostrará las trayectorias reales, las estimadas por EKF y UKF, y los puntos de medición. Podrás observar cómo ambos filtros intentan seguir la trayectoria real a pesar del ruido en las mediciones. En general, el UKF debería mostrar una estimación más suave y precisa, especialmente si las no linealidades de la función h_radar fueran más pronunciadas o el ruido de medición fuera mayor.

🔮 Conclusiones y Próximos Pasos

Hemos explorado en detalle los Filtros de Kalman Extendidos (EKF) y los Filtros de Kalman Inodoros (UKF), dos herramientas esenciales para la estimación de estados en sistemas no lineales. Mientras que el EKF se basa en una aproximación lineal, el UKF utiliza la transformación inodora para propagar puntos sigma, ofreciendo una mayor robustez y precisión en escenarios con fuertes no linealidades, a costa de un mayor costo computacional.

La elección entre EKF y UKF debe considerar la naturaleza de las no linealidades de tu sistema, los recursos computacionales disponibles y la precisión requerida para tu aplicación específica de Machine Learning o robótica.

¿Qué puedes explorar a continuación?

• Filtros de Partículas (Particle Filters): Para sistemas con no linealidades muy fuertes o distribuciones de ruido no gaussianas, los filtros de partículas pueden ofrecer una solución aún más robusta, aunque con un costo computacional significativamente mayor.
• Implementación de Jacobianos Analíticos: En el EKF, reemplazar la aproximación numérica de los Jacobianos por sus expresiones analíticas puede mejorar tanto la precisión como la eficiencia.
• Fusión de Sensores: Aplica EKF/UKF para combinar datos de múltiples sensores (GPS, IMU, LiDAR, cámara) para obtener una estimación de estado más precisa.
• Estimación de Parámetros: Utiliza estos filtros no solo para estimar el estado del sistema, sino también para estimar parámetros desconocidos del modelo, tratándolos como parte del estado extendido.
• Modelos de Aprendizaje por Refuerzo: Los filtros de Kalman pueden ser utilizados en el contexto del aprendizaje por refuerzo para estimar el estado de un entorno parcialmente observable.

La estimación de estados es un campo vasto y fundamental. Dominar EKF y UKF te proporcionará una base sólida para abordar muchos problemas complejos en el ámbito de la Inteligencia Artificial y más allá.