filtro kalman 2d en python

kalman filter opencv python

Question

Mi entrada es 2d (x, y) series temporales de un punto que se mueve en una pantalla para un software de seguimiento. Tiene un poco de ruido que quiero eliminar usando el filtro de Kalman. ¿Alguien me puede indicar un código python para el filtro Kalman 2d? En Scipy Cookbook solo encontré un ejemplo 1: http://www.scipy.org/Cookbook/KalmanFiltering Vi que hay una implementación para el filtro de Kalman en OpenCV, pero no pude encontrar ejemplos de código. ¡Gracias!

Answer 1

Aquí está mi implementación del filtro de Kalman basado en las ecuaciones dadas en wikipedia . Tenga en cuenta que mi comprensión de los filtros de Kalman es muy rudimentaria, por lo que existen formas más probables de mejorar este código. (Por ejemplo, sufre el problema de inestabilidad numérica que se analiza here . Según tengo entendido, esto solo afecta a la estabilidad numérica cuando Q , el ruido de movimiento, es muy pequeño. En la vida real, el ruido generalmente no es pequeño, tan afortunadamente ( al menos para mi implementación) en la práctica, la inestabilidad numérica no aparece.

En el siguiente ejemplo, kalman_xy asume que el vector de estado es un número de 4 tuplas: 2 números para la ubicación y 2 números para la velocidad. Las matrices F y H se han definido específicamente para este vector de estado: si x es un estado de 4 tuplas, entonces

new_x = F * x
position = H * x

Luego llama a kalman , que es el filtro de Kalman generalizado. Es general en el sentido de que aún es útil si desea definir un vector de estado diferente, tal vez una tupla de 6 que represente la ubicación, la velocidad y la aceleración. Solo tiene que definir las ecuaciones de movimiento proporcionando los valores F y H apropiados.

import numpy as np
import matplotlib.pyplot as plt

def kalman_xy(x, P, measurement, R,
              motion = np.matrix(''0. 0. 0. 0.'').T,
              Q = np.matrix(np.eye(4))):
    """
    Parameters:    
    x: initial state 4-tuple of location and velocity: (x0, x1, x0_dot, x1_dot)
    P: initial uncertainty convariance matrix
    measurement: observed position
    R: measurement noise 
    motion: external motion added to state vector x
    Q: motion noise (same shape as P)
    """
    return kalman(x, P, measurement, R, motion, Q,
                  F = np.matrix(''''''
                      1. 0. 1. 0.;
                      0. 1. 0. 1.;
                      0. 0. 1. 0.;
                      0. 0. 0. 1.
                      ''''''),
                  H = np.matrix(''''''
                      1. 0. 0. 0.;
                      0. 1. 0. 0.''''''))

def kalman(x, P, measurement, R, motion, Q, F, H):
    ''''''
    Parameters:
    x: initial state
    P: initial uncertainty convariance matrix
    measurement: observed position (same shape as H*x)
    R: measurement noise (same shape as H)
    motion: external motion added to state vector x
    Q: motion noise (same shape as P)
    F: next state function: x_prime = F*x
    H: measurement function: position = H*x

    Return: the updated and predicted new values for (x, P)

    See also http://en.wikipedia.org/wiki/Kalman_filter

    This version of kalman can be applied to many different situations by
    appropriately defining F and H 
    ''''''
    # UPDATE x, P based on measurement m    
    # distance between measured and current position-belief
    y = np.matrix(measurement).T - H * x
    S = H * P * H.T + R  # residual convariance
    K = P * H.T * S.I    # Kalman gain
    x = x + K*y
    I = np.matrix(np.eye(F.shape[0])) # identity matrix
    P = (I - K*H)*P

    # PREDICT x, P based on motion
    x = F*x + motion
    P = F*P*F.T + Q

    return x, P

def demo_kalman_xy():
    x = np.matrix(''0. 0. 0. 0.'').T 
    P = np.matrix(np.eye(4))*1000 # initial uncertainty

    N = 20
    true_x = np.linspace(0.0, 10.0, N)
    true_y = true_x**2
    observed_x = true_x + 0.05*np.random.random(N)*true_x
    observed_y = true_y + 0.05*np.random.random(N)*true_y
    plt.plot(observed_x, observed_y, ''ro'')
    result = []
    R = 0.01**2
    for meas in zip(observed_x, observed_y):
        x, P = kalman_xy(x, P, meas, R)
        result.append((x[:2]).tolist())
    kalman_x, kalman_y = zip(*result)
    plt.plot(kalman_x, kalman_y, ''g-'')
    plt.show()

demo_kalman_xy()

Los puntos rojos muestran las medidas de posición ruidosas, la línea verde muestra las posiciones predichas de Kalman.