web-dev-qa-db-ja.com

pythonのkalman 2dフィルター

私の入力は、トラッカーソフトウェアの画面上を移動するドットの2D（x、y）時系列です。カルマンフィルターを使用して削除したいノイズがあります。誰かがpython Kalman 2dフィルターのコードを指すことができますか？scipy cookbookで1dの例しか見つかりませんでした： http://www.scipy.org/Cookbook/KalmanFiltering OpenCVにKalmanフィルターの実装があることを見ましたが、コード例が見つかりませんでした。

python2dkalman-filter
32
Noam Peled

wikipediaで与えられた式 に基づいたカルマンフィルターの実装です。カルマンフィルターについての私の理解は非常に初歩的なものであるため、このコードを改善する方法はほとんどあることに注意してください。 （例えば、それは here で議論された数値不安定性の問題に苦しんでいます。私が理解するように、これはモーションノイズであるQが非常に小さい場合にのみ数値安定性に影響します。人生では、ノイズは通常小さくないため、幸いなことに（少なくとも私の実装では）実際には数値的な不安定性は現れません。）

以下の例では、kalman_xyは、状態ベクトルが4タプルであると想定しています。位置に2つの数字、速度に2つの数字を使用します。 FおよびH行列は、この状態ベクトル用に特別に定義されています：xが4タプル状態の場合、

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

enter image description here

赤い点はノイズの多い位置の測定値を示し、緑の線はカルマンの予測位置を示します。

54
unutbu