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

问题描述：

[编辑] @Claudio的答案给了我一个关于如何过滤掉异常值的非常好的提示。我确实想开始在我的数据上使用卡尔曼滤波器。所以我改变了下面的示例数据，以便它具有微妙的变化噪音，这并不是那么极端（我也看到很多）。如果其他人可以给我一些关于如何在我的数据上使用PyKalman的方向，那将是非常棒的。 [/编辑]如何在Python中使用卡尔曼滤波器来获取位置数据？

对于一个机器人项目，我试图用相机跟踪风筝。我使用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]])

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

您可能需要过滤器，但我不确定您是否需要卡尔曼滤波器。除非你确定你需要卡尔曼滤波器，否则我会建议询问在这里使用什么样的滤波器：http://dsp.stackexchange.com/ –

不是你的问题的答案;但是移除3-sigma以外的值将会消除所有发布的噪声值，而不是其他任何值。 – Ben

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

答

TL; DR，请参阅底部的代码和图片。

我认为一个卡尔曼滤波器可以在你的应用程序中运行得很好，但是它需要更多关于风筝动力学/物理学的思考。我会强烈推荐阅读this webpage。我与作者没有关系，也没有知识，但是我花了大概一天的时间试图让我的头卡尔曼过滤器，这个网页真的让我点击。

简而言之;对于一个线性的系统，它具有已知的动态特性（即如果你知道状态和输入，你可以预测未来的状态），它提供了一个最佳的方式来组合你知道的系统来估计它的真实状态。聪明位（这是由你描述它网页上看到的所有矩阵代数照顾）是如何最佳地结合了两条信息您有：

测量（这是受“测量噪声“，即传感器不完美）
动态（即你如何相信国家的发展受制于输入，这受到”过程噪音“的影响，这只是说明你的模型完全不符合实际的一种方式）。

你指定你关于这些如何确保（通过共同方差矩阵[R和分别给出），以及卡尔曼增益决定多少，你应该相信你的模型（即你目前对你所在州的估计），以及你应该相信你的测量结果。

没有进一步的让我们建立一个简单的风筝模型。我在下面提出的是一个非常简单的可能模型。你也许知道更多关于风筝的动态，所以可以创建一个更好的。

让我们把风筝作为粒子（显然是一个简化，真正的风筝是一个扩展的身体，所以有一个方向的3个维），其中有四个州为方便起见，我们可以在一个状态向量写：

X = [X，x_dot，Y，y_dot]，

其中x和y是位置，并且_dot的是在每个这些方向的速度。从你的问题我假设有两个（潜在的噪声）的测量，我们可以在一个测量向量写：

ž = [X，Y]，

我们可以写下来测量矩阵（ħ讨论here，和observation_matrices在pykalman库）：

ž = ħ X =>ħ = [[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” 是添E-一步。我们假设（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的）
系统动力学（和状态的当前估计）

上述模型中捕获的动力学非常简单。从字面上看，他们认为位置会根据当前速度（以一种明显的，物理上合理的方式）进行更新，并且速度保持不变（这显然不是物理上的真实，但捕捉了速度应该缓慢变化的直觉）。

如果我们认为估计的状态应该是平滑的，实现这一目标的一个方法是说，我们已经在我们的测量结果比我们的动态信心不足（即我们有一个更高的observation_covariance，相对于我们state_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

我们可以将其与异常值检测/消除方法进行比较。如果风筝模型假设没有动态（我们没有介绍速度状态），我认为卡尔曼滤波器纯粹是最大似然估计（平均位置），假设测量噪声为零均值和正态分布。然而，这将失去如果风筝快速向下移动的信息，那么在下一个区间内它可能会进一步下降。在上面实施的偏差大的偏差并没有完全忽略（见“测量门控”以帮助实现这一点）。 – kabdulla

感谢您的精心解答。这花了我几个小时，但我认为我现在更了解卡尔曼滤波器的想法。还有一个问题;如果我想使平滑度变得更强一些，以便线条比现在更平滑，我该怎么做？ – kramer65

不用担心。希望它不是太精细;我对卡尔曼滤波器本人相当陌生，所以试图保持简单，但现在我更加热衷于卡尔曼滤波器，因此可能会被带走！你所建议的应该是非常简单的。我将在这方面进一步详细阐述答案。 – kabdulla

答

从我所看到的使用卡尔曼滤波可能不是您的情况下正确的工具。

这样做怎么办？：

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

这里输出：

一些更多的，我已经得到了来自上面的代码源见here。

这的确是个好主意！我一定会用这个技巧。除此之外，我也想使用卡尔曼滤波器。你能否给我一个我如何使用卡尔曼滤波器的例子？ – kramer65

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

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

相关推荐