filtro Kalman 2d en Python

Aquí está mi implementación del filtro de Kalman basada en las ecuaciones dadas en Wikipedia . Tenga en cuenta que mi comprensión de los filtros de Kalman es muy rudimentaria, por lo que es muy probable que existan formas de mejorar este código. (Por ejemplo, sufre el problema de inestabilidad numérica discutido aquí . Según tengo entendido, esto solo afecta la estabilidad numérica cuando Qel ruido de movimiento es muy pequeño. En la vida real, el ruido generalmente no es pequeño, así que afortunadamente (en al menos para mi implementación) en la práctica la inestabilidad numérica no aparece.)

En el siguiente ejemplo, kalman_xyse supone que el vector de estado es una tupla de 4: 2 números para la ubicación y 2 números para la velocidad. Las matrices Fy Hse han definido específicamente para este vector de estado: Si xes 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 sigue siendo ú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. Sólo tienes que definir las ecuaciones de movimiento proporcionando las apropiadas Fy H.

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 mediciones de posición ruidosas, la línea verde muestra las posiciones predichas por Kalman.

Dec 16 '2012 17:12 unutbu