在Python卡尔曼滤波2D [英] kalman 2d filter in python

问题描述

我的输入是二维（X，Y）的时间序列的点移动在屏幕上的一个跟踪器软件。它有一些噪音，我想用卡尔曼滤波器滤除。是否有人可以点我蟒蛇code卡尔曼滤波2D？ 在SciPy的食谱，我发现只是一维的例子： <一href="http://www.scipy.org/Cookbook/KalmanFiltering">http://www.scipy.org/Cookbook/KalmanFiltering 我看到有实现在OpenCV的卡尔曼滤波，但找不到code的例子。 谢谢！

My input is 2d (x,y) time series of a dot moving on a screen for a tracker software. It has some noise I want to remove using Kalman filter. Does someone can point me for a python code for Kalman 2d filter? In scipy cookbook I found only a 1d example: http://www.scipy.org/Cookbook/KalmanFiltering I saw there is implementation for Kalman filter in OpenCV, but couldn't find code examples. Thanks!

推荐答案

下面是我实现卡尔曼滤波器的基础上给出的方程在维基百科的。请注意，我卡尔曼滤波的理解是非常简陋所以最有可能的方式来改善这一code。 （例如，它遭受的数值不稳定问题讨论这里。据我了解，这只会影响的数值时的稳定性问：，运动噪音，是非常小的。在现实生活中，噪声通常是不小的，所以幸运的是（至少对我实施）在实践中的数值不稳定不露面。）

Here is my implementation of the Kalman filter based on the equations given on wikipedia. Please be aware that my understanding of Kalman filters is very rudimentary so there are most likely ways to improve this code. (For example, it suffers from the numerical instability problem discussed here. As I understand it, this only affects the numerical stability when Q, the motion noise, is very small. In real life, the noise is usually not small, so fortunately (at least for my implementation) in practice the numerical instability does not show up.)

在下面的例子中， kalman_xy 假设状态向量是一个四元组：2号的位置，和2号的速度。 该 F 和 ^ h 矩阵已经明确定义了这个状态向量：如果 X 是一个4元组的状态，那么

In the example below, kalman_xy assumes the state vector is a 4-tuple: 2 numbers for the location, and 2 numbers for the velocity. The F and H matrices have been defined specifically for this state vector: If x is a 4-tuple state, then

new_x = F * x
position = H * x

，然后调用卡尔曼，这是广义卡尔曼滤波器。也许是6元组重presenting的位置，速度和加速度 - 如果你希望定义一个不同的状态向量是普遍的意义上，它仍然是有用的。您只需通过提供相应的来定义运动方程˚F和 ^ h 。

It then calls kalman, which is the generalized Kalman filter. It is general in the sense it is still useful if you wish to define a different state vector -- perhaps a 6-tuple representing location, velocity and acceleration. You just have to define the equations of motion by supplying the appropriate F and 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()

红点表示嘈杂的位置测量，绿线显示了卡尔曼predicted位置。

The red dots show the noisy position measurements, the green line shows the Kalman predicted positions.

这篇关于在Python卡尔曼滤波2D的文章就介绍到这了，希望我们推荐的答案对大家有所帮助，也希望大家多多支持IT屋！