logo标签列表

Python中的卡尔曼2D滤波器

pythonnumpyscipysmoothingkalman-filter
46

我的输入是一个2D(x,y)的时间序列，表示跟踪软件屏幕上一点的运动。由于存在一些噪声，我想使用卡尔曼滤波器来去除这些噪声。请问是否有人能为我提供Python代码实现卡尔曼2D滤波器？

在Scipy Cookbook中，我只找到了一个1D的示例：http://www.scipy.org/Cookbook/KalmanFiltering

我看到OpenCV中有卡尔曼滤波器的实现，但是找不到代码示例。谢谢！

1
我对这个话题并不是专家，但在课堂上我们总是将过滤器分别应用于每一行和每一列的二维空间。你试过了吗？也许这样会立即改善你的结果。 - erikbstack
2
我猜如果您没有指定，您需要将1D示例推广一下。因为您没有说明应该使用numpy的Python（我只是猜测这一点）。否则，我会向您指出类似于Matlab代码的OSS，http://www.mathworks.com/matlabcentral/fileexchange/14243-2d-target-tracking-using-kalman-filter，可以用于scipy。在您提供的同一链接中，还有一个源链接，可以带您了解作者收集的关于Klaman过滤器的所有信息 http://www.cs.unc.edu/~welch/kalman/。 - Yauhen Yakimovich
1
erikb85 - 我尝试将其分别应用于每行和每列，我的结果得到了改善。谢谢！我对卡尔曼滤波非常陌生，不确定这是否是正确的方法。 Yauhen Yakimovich - 谢谢，我从http://www.cs.ubc.ca/~murphyk/Software/Kalman/kalman.html下载了2D跟踪的Matlab代码。它还有一个很好的EM学习器来过滤参数。我不确定我是否会转向Matlab或将所有代码翻译成Python... - Noam Peled
3
我在这里找到了学习使用卡尔曼滤波器的最简单方法。https://stackoverflow.com/a/53017661/7060530 - Sayed Mohsin Reza
2个回答
67
这是我基于维基百科上给出的方程式实现的卡尔曼滤波器。请注意，我的卡尔曼滤波理解非常基础，所以很可能有改进此代码的方法。（例如，它存在数字不稳定性问题，在此处讨论。据我了解，只有当运动噪声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

红色的点显示嘈杂的位置测量，绿线显示卡尔曼预测的位置。
2
计算初始不确定性，例如 P = np.matrix(np.eye(4))*1000。为什么要乘以1000？ - Sophia
1
在许多使用“*”的地方，应该有矩阵乘法吧？ - Sandu Ursu
@SanduUrsu 当 * 运算符的参数类型为 np.matrix（而不是 np.array）时，将执行矩阵乘法。但为了安全起见，可以使用 @ 运算符来确保在任何情况下都进行矩阵乘法。 - hyperspasm
在示例demo_kalman_xy()中，R应该是一个2x2（测量噪声协方差）矩阵，例如R = np.matrix([[r,0], [0,r]])。在kalman_xy()中，Q的默认值也可能太高，以至于很难看到调整R的效果。 - hyperspasm
1
为了我的一个项目，我需要创建时间序列建模的间隔，并且为了使过程更加高效，我创建了tsmoothie：一种用向量化方式进行时间序列平滑和异常值检测的Python库。
它提供了不同的平滑算法以及计算间隔的可能性。
在KalmanSmoother的情况下，您可以将不同的组件（水平、趋势、季节性、长期季节性）组合在一起对曲线进行平滑处理。
import numpy as np
import matplotlib.pyplot as plt
from tsmoothie.smoother import *
from tsmoothie.utils_func import sim_randomwalk

# generate 3 randomwalks timeseries of lenght 100
np.random.seed(123)
data = sim_randomwalk(n_series=3, timesteps=100, 
                      process_noise=10, measure_noise=30)

# operate smoothing
smoother = KalmanSmoother(component='level_trend', 
                          component_noise={'level':0.1, 'trend':0.1})
smoother.smooth(data)

# generate intervals
low, up = smoother.get_intervals('kalman_interval', confidence=0.05)

# plot the first smoothed timeseries with intervals
plt.figure(figsize=(11,6))
plt.plot(smoother.smooth_data[0], linewidth=3, color='blue')
plt.plot(smoother.data[0], '.k')
plt.fill_between(range(len(smoother.data[0])), low[0], up[0], alpha=0.3)

enter image description here

我还要指出的是，tsmoothie可以以向量化的方式对多个时间序列进行平滑处理。
1
你的示例仅为1维数据。问题明确要求2维数据。 - LudvigH
在我的情况下，x代表时间（假设是递增的），y代表系列达到的值，因此2D...它是上面答案中报告的相同情况，其中observed_x和observed_y是两个递增的量。 - Marco Cerliani
这里的X和Y只描述了一个空间（尽管具有不同的特征，如坐标）... 多维卡尔曼实现（例如2个点 - 每个点在共同条件下的自己的空间中）似乎相当难以形式化为某种算法... 或者我错了吗？ - JeeyCi
网页内容由stack overflow 提供, 点击上面的
可以查看英文原文，
原文链接