web-dev-qa-db-fra.com

filtre kalman 2d dans python

Mon entrée est une série temporelle 2d (x, y) d'un point se déplaçant sur un écran pour un logiciel de suivi. Il y a du bruit que je veux supprimer en utilisant le filtre de Kalman. Quelqu'un peut-il me pointer pour un code python pour le filtre Kalman 2d? Dans le livre de recettes scipy, je n'ai trouvé qu'un exemple 1d: http://www.scipy.org/Cookbook/KalmanFiltering J'ai vu qu'il existe une implémentation du filtre Kalman dans OpenCV, mais je n'ai pas trouvé d'exemples de code. Merci!

python2dkalman-filter
32
Noam Peled

Voici mon implémentation du filtre de Kalman basé sur les équations données sur wikipedia . Veuillez noter que ma compréhension des filtres de Kalman est très rudimentaire, il existe donc probablement des moyens d'améliorer ce code. (Par exemple, il souffre du problème d'instabilité numérique discuté ici . Si je comprends bien, cela n'affecte la stabilité numérique que lorsque Q, le bruit de mouvement, est très faible. En réalité la vie, le bruit n'est généralement pas faible, donc heureusement (au moins pour ma mise en œuvre) en pratique l'instabilité numérique n'apparaît pas.)

Dans l'exemple ci-dessous, kalman_xy suppose que le vecteur d'état est un quadruple: 2 nombres pour l'emplacement et 2 nombres pour la vitesse. Les matrices F et H ont été définies spécifiquement pour ce vecteur d'état: Si x est un état à 4 tuples, alors

new_x = F * x
position = H * x

Il appelle ensuite kalman, qui est le filtre de Kalman généralisé. Il est général dans le sens où il est toujours utile si vous souhaitez définir un vecteur d'état différent - peut-être un 6-Tuple représentant l'emplacement, la vitesse et l'accélération. Il suffit de définir les équations du mouvement en fournissant les F et H appropriés.

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()

enter image description here

Les points rouges montrent les mesures de position bruyantes, la ligne verte montre les positions prédites de Kalman.

54
unutbu