kalman在python中的2d過濾器

Question

我的輸入是在跟蹤器軟件的屏幕上移動的點的2d（x，y）時間序列。它有一些我想用卡爾曼濾波器消除的噪音。有人可以指示我爲卡爾曼2d過濾器的Python代碼？ 在scipy食譜中，我發現只有一個例子： http://www.scipy.org/Cookbook/KalmanFiltering 我看到在OpenCV中有卡爾曼濾波實現，但找不到代碼示例。 謝謝！

Answer 1

這裏是我基於equations given on wikipedia的卡爾曼濾波器的實現。請注意，我對卡爾曼濾波器的理解很簡單，所以最有可能的方法是改進這些代碼。 （例如，它遇到了討論的數值不穩定性問題here據我所知，這隻會影響運動噪聲非常小時的數值穩定性，在現實生活中，噪聲通常不小，所以幸運的是（至少在我的實現中）在實踐中，數字不穩定性不會顯示出來。）

在下面的例子中，kalman_xy假設狀態向量是一個4元組：位置爲2個數字，速度爲2個數字。 的F和H矩陣已被專門定義爲這種狀態矢量：如果x是一個四元組的狀態，然後

new_x = F * x 
position = H * x 

然後它調用kalman，這是廣義卡爾曼濾波器。如果你想定義一個不同的狀態向量 - 也許是一個代表位置，速度和加速度的6元組，那麼它在一般意義上仍然有用。您只需通過提供相應的F和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() 

紅色點表示嘈雜位置測量，綠線表示卡爾曼預測位置。