python中的卡尔曼二维过滤器

Posted 2023-03-12

【中文标题】python中的卡尔曼二维过滤器

【英文标题】：kalman 2d filter in python

【发布时间】：2012-12-03 19:56:33

【问题描述】：

我的输入是跟踪器软件在屏幕上移动的点的 2d (x,y) 时间序列。它有一些我想使用卡尔曼滤波器去除的噪音。有人可以为我指出卡尔曼 2d 滤波器的 python 代码吗？ 在 scipy 食谱中，我只找到了一个 1d 示例： http://www.scipy.org/Cookbook/KalmanFiltering 我看到 OpenCV 中有卡尔曼滤波器的实现，但找不到代码示例。 谢谢！

【问题讨论】：

我当然不是这个主题的专家，但在课堂上，我们总是将过滤器应用于二维空间，并将其分别应用于每一行和每一列。你试过吗？也许它已经可以改善您的结果了。

我猜你必须概括一维示例。由于您没有指定它应该在 python 中使用 numpy（我只是在猜这个）否则我只会指出您可以用于 scipy 的类似 OSS 的 matlab 代码mathworks.com/matlabcentral/fileexchange/…。在您提供的同一个链接中，源中的另一个链接带来了 sn-p cs.unc.edu/~welch/kalman

的作者在 Klaman 过滤中收集的所有类型的信息

erikb85 - 我确实尝试将它分别应用于每一行和每一列，并且我的结果得到了改进。谢谢！我对卡尔曼滤波很陌生，不确定它是否是正确的方法。 Yauhen Yakimovich - 谢谢，我从cs.ubc.ca/~murphyk/Software/Kalman/kalman.html 下载了二维跟踪的 Matlab 代码。它还有一个很好的过滤器参数的 EM 学习器。我不确定我是否会转移到 Matlab 或将所有代码翻译成 Python...

我在这里找到的了解卡尔曼滤波器使用的最简单方法。 ***.com/a/53017661/7060530

【参考方案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)

我还指出tsmoothie可以以矢量化的方式对多个时间序列进行平滑

【讨论】：

您的示例仅为 1d。该问题专门针对二维数据提出

在我的情况下，x 是时间（假设正在增加），y 是系列达到的值，所以 2D ......这与上面的答案中报告的情况相同，其中 observed_x 和 observed_y 是二增数量

【参考方案2】：

这是我基于equations given on 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()

红点显示噪声位置测量，绿线显示卡尔曼预测位置。

【讨论】：

计算初始不确定性，如P = np.matrix(np.eye(4))*1000。为什么要乘以 1000？

在很多地方用“*”的地方不应该有矩阵乘法吗？

@SanduUrsu 当 * 运算符的参数类型为 np.matrix（与 np.array 相对）时执行矩阵乘法。不过为了安全起见，@ 运算符可用于确保在任何一种情况下都可以进行矩阵乘法。

在示例中demo_kalman_xy() R 实际上应该是一个 2x2（测量噪声协方差）矩阵，例如 R = np.matrix([[r,0], [0,r]])。 kalman_xy()中Q的默认值也可能太高了，很难看到调整R的效果。