如何在 Python 中使用卡尔曼滤波器获取位置数据？

Posted 2023-02-14

【中文标题】如何在 Python 中使用卡尔曼滤波器获取位置数据？

【英文标题】：How to use Kalman filter in Python for location data?

【发布时间】：2017-09-08 17:05:00

【问题描述】：

[编辑] @Claudio 的回答给了我一个关于如何过滤异常值的非常好的提示。不过，我确实想开始对我的数据使用卡尔曼滤波器。因此，我更改了下面的示例数据，使其具有不那么极端的细微变化噪声（我也经常看到）。如果其他人能给我一些关于如何在我的数据上使用 PyKalman 的指导，那就太好了。 [/编辑]

对于一个机器人项目，我正在尝试用相机跟踪空中的风筝。我正在用 Python 编程，并在下面粘贴了一些嘈杂的位置结果（每个项目还包含一个日期时间对象，但为了清楚起见，我将它们省略了）。

[           # X     Y 
    'loc': (399, 293),
    'loc': (403, 299),
    'loc': (409, 308),
    'loc': (416, 315),
    'loc': (418, 318),
    'loc': (420, 323),
    'loc': (429, 326),  # <== Noise in X
    'loc': (423, 328),
    'loc': (429, 334),
    'loc': (431, 337),
    'loc': (433, 342),
    'loc': (434, 352),  # <== Noise in Y
    'loc': (434, 349),
    'loc': (433, 350),
    'loc': (431, 350),
    'loc': (430, 349),
    'loc': (428, 347),
    'loc': (427, 345),
    'loc': (425, 341),
    'loc': (429, 338),  # <== Noise in X
    'loc': (431, 328),  # <== Noise in X
    'loc': (410, 313),
    'loc': (406, 306),
    'loc': (402, 299),
    'loc': (397, 291),
    'loc': (391, 294),  # <== Noise in Y
    'loc': (376, 270),
    'loc': (372, 272),
    'loc': (351, 248),
    'loc': (336, 244),
    'loc': (327, 236),
    'loc': (307, 220)
]

我首先想到的是手动计算异常值，然后简单地从数据中实时删除它们。然后我阅读了卡尔曼滤波器以及它们如何专门用于平滑噪声数据。 因此，经过一番搜索，我找到了PyKalman library，这似乎非常适合。因为我有点迷失在整个卡尔曼滤波器术语中，所以我阅读了 wiki 和其他一些关于卡尔曼滤波器的页面。我了解了卡尔曼滤波器的一般概念，但我真的不知道如何将它应用到我的代码中。

在PyKalman docs我找到了下面的例子：

>>> from pykalman import KalmanFilter
>>> import numpy as np
>>> kf = KalmanFilter(transition_matrices = [[1, 1], [0, 1]], observation_matrices = [[0.1, 0.5], [-0.3, 0.0]])
>>> measurements = np.asarray([[1,0], [0,0], [0,1]])  # 3 observations
>>> kf = kf.em(measurements, n_iter=5)
>>> (filtered_state_means, filtered_state_covariances) = kf.filter(measurements)
>>> (smoothed_state_means, smoothed_state_covariances) = kf.smooth(measurements)

我只是用以下观察代替了我自己的观察：

from pykalman import KalmanFilter
import numpy as np
kf = KalmanFilter(transition_matrices = [[1, 1], [0, 1]], observation_matrices = [[0.1, 0.5], [-0.3, 0.0]])
measurements = np.asarray([(399,293),(403,299),(409,308),(416,315),(418,318),(420,323),(429,326),(423,328),(429,334),(431,337),(433,342),(434,352),(434,349),(433,350),(431,350),(430,349),(428,347),(427,345),(425,341),(429,338),(431,328),(410,313),(406,306),(402,299),(397,291),(391,294),(376,270),(372,272),(351,248),(336,244),(327,236),(307,220)])
kf = kf.em(measurements, n_iter=5)
(filtered_state_means, filtered_state_covariances) = kf.filter(measurements)
(smoothed_state_means, smoothed_state_covariances) = kf.smooth(measurements)

但这并没有给我任何有意义的数据。例如，smoothed_state_means 变为：

>>> smoothed_state_means
array([[-235.47463353,   36.95271449],
       [-354.8712597 ,   27.70011485],
       [-402.19985301,   21.75847069],
       [-423.24073418,   17.54604304],
       [-433.96622233,   14.36072376],
       [-443.05275258,   11.94368163],
       [-446.89521434,    9.97960296],
       [-456.19359012,    8.54765215],
       [-465.79317394,    7.6133633 ],
       [-474.84869079,    7.10419182],
       [-487.66174033,    7.1211321 ],
       [-504.6528746 ,    7.81715451],
       [-506.76051587,    8.68135952],
       [-510.13247696,    9.7280697 ],
       [-512.39637431,   10.9610031 ],
       [-511.94189431,   12.32378146],
       [-509.32990832,   13.77980587],
       [-504.39389762,   15.29418648],
       [-495.15439769,   16.762472  ],
       [-480.31085928,   18.02633612],
       [-456.80082586,   18.80355017],
       [-437.35977492,   19.24869224],
       [-420.7706184 ,   19.52147918],
       [-405.59500937,   19.70357845],
       [-392.62770281,   19.8936389 ],
       [-388.8656724 ,   20.44525168],
       [-361.95411607,   20.57651509],
       [-352.32671579,   20.84174084],
       [-327.46028214,   20.77224385],
       [-319.75994982,   20.9443245 ],
       [-306.69948771,   21.24618955],
       [-287.03222693,   21.43135098]])

比我更聪明的灵魂能给我一些正确方向的提示或例子吗？欢迎所有提示！

【问题讨论】：

您可能需要一个过滤器，但我不确定您是否需要一个卡尔曼过滤器。除非您确定需要卡尔曼滤波器，否则我建议您在此处询问使用哪种排序过滤：dsp.stackexchange.com

不是您的问题的答案；但是删除 3-sigma 之外的值将删除您发布的所有噪声值，仅此而已。

在我（微弱）的理解中，卡尔曼滤波器可以调整（不完美）物理/数学模型的预测与实际（嘈杂）测量之间的差异。 — 在您的问题陈述中，我无法识别位置的预测模型，所以我想知道卡尔曼滤波器是否可以帮助您。

@gboffi - 根据我对卡尔曼滤波器的理解，它需要一系列测量来平滑它，以便它可以用于 A) 结果更接近现实，因为噪声是或多或少抵消 B) 扩展测量点，以便可以对前面的点进行预测。还是我在这里完全错了？

您可能想查看我的开源书籍“Python 中的卡尔曼和贝叶斯滤波器”。它包括非常相似的项目。我不使用 PyKalman，而是使用我自己的库 FilterPy，您可以使用 pip 或 conda 安装它。对不起，如果这看起来像广告，但这本书确实回答了你的问题。 github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python

【参考方案1】：

据我所见，使用卡尔曼滤波可能不是适合您的工具。

这样做怎么样？ ：

lstInputData = [
    [346, 226 ],
    [346, 211 ],
    [347, 196 ],
    [347, 180 ],
    [350, 2165],  ## noise
    [355, 154 ],
    [359, 138 ],
    [368, 120 ],
    [374, -830],  ## noise
    [346, 90  ],
    [349, 75  ],
    [1420, 67 ],  ## noise
    [357, 64  ],
    [358, 62  ]
]

import pandas as pd
import numpy as np
df = pd.DataFrame(lstInputData)
print( df )
from scipy import stats
print ( df[(np.abs(stats.zscore(df)) < 1).all(axis=1)] )

这里是输出：

      0     1
0    346   226
1    346   211
2    347   196
3    347   180
4    350  2165
5    355   154
6    359   138
7    368   120
8    374  -830
9    346    90
10   349    75
11  1420    67
12   357    64
13   358    62
      0    1
0   346  226
1   346  211
2   347  196
3   347  180
5   355  154
6   359  138
7   368  120
9   346   90
10  349   75
12  357   64
13  358   62

请参阅here 了解更多信息以及我从中获得上述代码的来源。

【讨论】：

这确实是个好主意！我肯定会使用这个技巧。除此之外，我还想使用卡尔曼滤波器。您能否举个例子说明如何使用卡尔曼滤波器？

@kramer65 我认为使用卡尔曼滤波的主题过于广泛，无法在此讨论。另外我不是卡尔曼滤波器专家，所以如果你不能接受我的答案并接受它，你将不得不等待其他答案。我在这里回复是因为您写道：“欢迎所有提示！” :)

【参考方案2】：

TL;DR，见底部的代码和图片。

我认为卡尔曼滤波器可以在您的应用程序中很好地工作，但它需要更多地考虑风筝的动力学/物理学。

我强烈推荐阅读this webpage。我与作者没有任何联系，也不了解作者，但我花了大约一天的时间试图了解卡尔曼滤波器，这个页面真的让我点击了。

简单地说；对于一个线性且具有已知动态的系统（即，如果您知道状态和输入，您可以预测未来状态），它提供了一种结合您对系统的了解来估计其真实状态的最佳方式。聪明的一点（你在描述它的页面上看到的所有矩阵代数都照顾到了）是它如何最佳地结合你所拥有的两条信息：

测量（受“测量噪声”影响，即传感器不完美）

动力学（即您认为状态如何根据输入演变，而输入受“过程噪声”影响，这只是一种说法，您的模型与现实不完全匹配）。

李>

您指定您对其中每一个的确定程度（分别通过协方差矩阵 R 和 Q）以及 卡尔曼增益 决定了你应该相信你的模型多少（即你当前对你的状态的估计），以及你应该相信你的测量结果的程度。

事不宜迟，让我们为您的风筝构建一个简单的模型。我在下面提出的是一个非常简单的可能模型。您或许对 Kite 的动力学有更多了解，因此可以创造出更好的动力学。

让我们将风筝视为一个粒子（显然是一种简化，真正的风筝是一个扩展体，因此具有 3 个维度的方向），它有四种状态，为了方便我们可以将其写成状态向量：

x = [x, x_dot, y, y_dot],

其中 x 和 y 是位置，_dot 是每个方向上的速度。根据您的问题，我假设有两个（可能是嘈杂的）测量值，我们可以将它们写在测量向量中：

z = [x, y],

我们可以记下测量矩阵（H在pykalman库中讨论了here和observation_matrices）：

z = Hx => H = [[1, 0, 0, 0], [0, 0, 1, 0]]

然后我们需要描述系统动力学。在这里，我将假设没有外力作用，并且风筝的运动没有阻尼（如果有更多的知识，您可能会做得更好，这有效地将外力和阻尼视为未知/未建模的干扰）。

在这种情况下，我们当前样本“k”中每个状态的动态作为先前样本“k-1”中状态的函数给出如下：

x(k) = x(k-1) + dt*x_dot(k-1)

x_dot(k) = x_dot(k-1)

y(k) = y(k-1) + dt*y_dot(k-1)

y_dot(k) = y_dot(k-1)

其中“dt”是时间步长。我们假设 (x, y) 位置根据当前位置和速度进行更新，并且速度保持不变。鉴于没有给出单位，我们只能说速度单位是这样的，我们可以从上面的等式中省略“dt”，即以 position_units/sample_interval 为单位（我假设您的测量样本处于恒定间隔）。我们可以将这四个方程总结为一个动态矩阵（F 在这里讨论，transition_matrices 在pykalman 库中）：

x(k) = Fx(k-1) => F = [[1, 1, 0, 0], [0, 1, 0, 0], [0, 0, 1, 1], [0, 0, 0, 1]]。

我们现在可以尝试在 python 中使用卡尔曼滤波器。从您的代码修改：

from pykalman import KalmanFilter
import numpy as np
import matplotlib.pyplot as plt
import time

measurements = np.asarray([(399,293),(403,299),(409,308),(416,315),(418,318),(420,323),(429,326),(423,328),(429,334),(431,337),(433,342),(434,352),(434,349),(433,350),(431,350),(430,349),(428,347),(427,345),(425,341),(429,338),(431,328),(410,313),(406,306),(402,299),(397,291),(391,294),(376,270),(372,272),(351,248),(336,244),(327,236),(307,220)])

initial_state_mean = [measurements[0, 0],
                      0,
                      measurements[0, 1],
                      0]

transition_matrix = [[1, 1, 0, 0],
                     [0, 1, 0, 0],
                     [0, 0, 1, 1],
                     [0, 0, 0, 1]]

observation_matrix = [[1, 0, 0, 0],
                      [0, 0, 1, 0]]

kf1 = KalmanFilter(transition_matrices = transition_matrix,
                  observation_matrices = observation_matrix,
                  initial_state_mean = initial_state_mean)

kf1 = kf1.em(measurements, n_iter=5)
(smoothed_state_means, smoothed_state_covariances) = kf1.smooth(measurements)

plt.figure(1)
times = range(measurements.shape[0])
plt.plot(times, measurements[:, 0], 'bo',
         times, measurements[:, 1], 'ro',
         times, smoothed_state_means[:, 0], 'b--',
         times, smoothed_state_means[:, 2], 'r--',)
plt.show()

它产生了以下结果，表明它在抑制噪声方面做得很合理（蓝色是 x 位置，红色是 y 位置，x 轴只是样本编号）。

假设您查看上面的情节并认为它看起来过于颠簸。你怎么能解决这个问题？如上所述，卡尔曼滤波器作用于两条信息：

测量值（在本例中是我们的两个状态，x 和 y） 系统动力学（以及当前的状态估计）

上述模型中捕获的动态非常简单。从字面上看，他们说位置将被当前速度更新（以一种明显的、物理上合理的方式），并且速度保持不变（这显然在物理上不正确，但抓住了我们的直觉，即速度应该缓慢变化）。

如果我们认为估计的状态应该更平滑，实现这一点的一种方法是说我们对测量的信心低于我们的动态（即，相对于我们的state_covariance，我们有更高的observation_covariance）。

从上面代码的末尾开始，将observation covariance 修复为之前估计的值的 10 倍，如图所示设置 em_vars 以避免重新估计观察协方差（请参阅here）

kf2 = KalmanFilter(transition_matrices = transition_matrix,
                  observation_matrices = observation_matrix,
                  initial_state_mean = initial_state_mean,
                  observation_covariance = 10*kf1.observation_covariance,
                  em_vars=['transition_covariance', 'initial_state_covariance'])

kf2 = kf2.em(measurements, n_iter=5)
(smoothed_state_means, smoothed_state_covariances)  = kf2.smooth(measurements)

plt.figure(2)
times = range(measurements.shape[0])
plt.plot(times, measurements[:, 0], 'bo',
         times, measurements[:, 1], 'ro',
         times, smoothed_state_means[:, 0], 'b--',
         times, smoothed_state_means[:, 2], 'r--',)
plt.show()

这产生了下面的图（测量为点，状态估计为虚线）。差异相当微妙，但希望您能看到它更平滑。

最后，如果你想在线使用这个拟合过滤器，你可以使用filter_update 方法。请注意，这里使用filter 方法而不是smooth 方法，因为smooth 方法只能应用于批量测量。更多here：

time_before = time.time()
n_real_time = 3

kf3 = KalmanFilter(transition_matrices = transition_matrix,
                  observation_matrices = observation_matrix,
                  initial_state_mean = initial_state_mean,
                  observation_covariance = 10*kf1.observation_covariance,
                  em_vars=['transition_covariance', 'initial_state_covariance'])

kf3 = kf3.em(measurements[:-n_real_time, :], n_iter=5)
(filtered_state_means, filtered_state_covariances) = kf3.filter(measurements[:-n_real_time,:])

print("Time to build and train kf3: %s seconds" % (time.time() - time_before))

x_now = filtered_state_means[-1, :]
P_now = filtered_state_covariances[-1, :]
x_new = np.zeros((n_real_time, filtered_state_means.shape[1]))
i = 0

for measurement in measurements[-n_real_time:, :]:
    time_before = time.time()
    (x_now, P_now) = kf3.filter_update(filtered_state_mean = x_now,
                                       filtered_state_covariance = P_now,
                                       observation = measurement)
    print("Time to update kf3: %s seconds" % (time.time() - time_before))
    x_new[i, :] = x_now
    i = i + 1

plt.figure(3)
old_times = range(measurements.shape[0] - n_real_time)
new_times = range(measurements.shape[0]-n_real_time, measurements.shape[0])
plt.plot(times, measurements[:, 0], 'bo',
         times, measurements[:, 1], 'ro',
         old_times, filtered_state_means[:, 0], 'b--',
         old_times, filtered_state_means[:, 2], 'r--',
         new_times, x_new[:, 0], 'b-',
         new_times, x_new[:, 2], 'r-')

plt.show()

下图显示了过滤方法的性能，包括使用filter_update 方法找到的 3 个点。点是测量值，虚线是过滤器训练期间的状态估计，实线是“在线”期间的状态估计。

还有时间信息（在我的笔记本电脑上）。

Time to build and train kf3: 0.0677888393402 seconds
Time to update kf3: 0.00038480758667 seconds
Time to update kf3: 0.000465154647827 seconds
Time to update kf3: 0.000463008880615 seconds

【讨论】：

我们可以将其与异常值检测/消除方法进行比较。如果风筝模型假设没有动力学（我们没有费心引入 _dot 速度状态），我认为卡尔曼滤波器将纯粹是最大似然估计（平均位置），假设测量噪声为零均值且正态分布。然而，这将失去这样的信息，即如果风筝正在快速向下移动，那么在下一个区间它可能会进一步下降。在上述实施的不利方面，大偏差并未完全被忽略（请参阅“测量门控”以帮助解决此问题）。

感谢您的详尽回答。我花了几个小时，但我想我现在对卡尔曼滤波器的想法要好得多。还有一个问题;如果我想让平滑度更强一些，让线条比现在更平滑，我该怎么做？

不用担心。希望不要太详细；我自己对卡尔曼滤波器还很陌生，所以试图保持简单，但我现在对它们很感兴趣，所以可能已经被带走了！你的建议应该很简单。我会在这个问题上进一步详细说明答案。

@kramer65 扩展答案对您有用吗？它有效地假设了您的测量值的更大方差（更多噪声），这意味着更加强调动态（在上述模型中，速度不会快速变化）。

太棒了，谢谢！最后一个问题。我正在使用它来实时确定我的风筝的飞行路径，但不幸的是，kf = kf.em(measurements, n_iter=5) 这条线花费了太长时间，无法每秒计算 25 次（这是视频片段的帧速率） .这可能是一个很长的尝试，但是您是否认为使用此功能可以提高性能？