kalman bộ lọc 2d trong python

Question

Đầu vào của tôi là chuỗi thời gian 2d (x, y) của một chấm di chuyển trên màn hình cho phần mềm theo dõi. Nó có một số tiếng ồn tôi muốn loại bỏ bằng cách sử dụng bộ lọc Kalman. Có ai đó có thể chỉ cho tôi một mã python cho bộ lọc Kalman 2d? Trong sách dạy nấu ăn scipy tôi chỉ tìm thấy một ví dụ 1d: http://www.scipy.org/Cookbook/KalmanFiltering Tôi thấy có triển khai bộ lọc Kalman trong OpenCV, nhưng không thể tìm thấy ví dụ mã. Cảm ơn!

Answer 1

Dưới đây là triển khai bộ lọc Kalman của tôi dựa trên equations given on wikipedia. Xin lưu ý rằng sự hiểu biết của tôi về các bộ lọc Kalman rất thô sơ nên có nhiều cách để cải thiện mã này. Như tôi đã hiểu, điều này chỉ ảnh hưởng đến độ ổn định số khi Q, tiếng ồn chuyển động rất nhỏ, trong thực tế, tiếng ồn thường không nhỏ, vì vậy may mắn thay (ví dụ: ít nhất là để thực hiện của tôi) trong thực tế sự bất ổn định số không hiển thị.)

Trong ví dụ dưới đây, kalman_xy giả sử vector trạng thái là 4-tuple: 2 số cho vị trí và 2 số cho vận tốc. Các ma trận F và H đã được xác định riêng cho vector trạng thái này: Nếu x là một trạng thái 4-tuple, sau đó

new_x = F * x 
position = H * x 

Sau đó nó gọi kalman, đó là bộ lọc Kalman khái quát hóa. Nó nói chung theo nghĩa nó vẫn hữu ích nếu bạn muốn định nghĩa một vectơ trạng thái khác nhau - có lẽ một bộ 6-đại diện cho vị trí, vận tốc và gia tốc. Bạn chỉ cần xác định phương trình chuyển động bằng cách cung cấp F và H thích hợp.

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

Các chấm đỏ thể hiện các phép đo vị trí ồn ào, dòng màu xanh lá cây cho thấy Kalman dự đoán vị trí.