如何在 Python 中使用卡尔曼滤波器获取位置数据? [英] How to use Kalman filter in Python for location data?
问题描述
@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)}, # <== X 中的噪声{'loc': (423, 328)},{'loc': (429, 334)},{'loc': (431, 337)},{'loc': (433, 342)},{'loc': (434, 352)}, # <== 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)}, # <== X 中的噪声{'loc': (431, 328)}, # <== X 中的噪声{'loc': (410, 313)},{'loc': (406, 306)},{'loc': (402, 299)},{'loc': (397, 291)},{'loc': (391, 294)}, # <== Y 中的噪声{'loc': (376, 270)},{'loc': (372, 272)},{'loc': (351, 248)},{'loc': (336, 244)},{'loc': (327, 236)},{'loc': (307, 220)}]我首先想到的是手动计算异常值，然后简单地从数据中实时删除它们.然后我阅读了卡尔曼滤波器以及它们如何专门用于平滑噪声数据.因此，经过一番搜索后，我找到了
假设你看了上面的图，觉得它看起来太颠簸了.你怎么能解决这个问题?如上所述，卡尔曼滤波器作用于两条信息:
- 测量(在我们的两个状态中，x 和 y)
- 系统动力学(和当前状态估计)
上述模型中捕获的动态非常简单.从字面上看，他们说位置将被当前速度更新(以一种明显的、物理上合理的方式)，并且速度保持不变(这显然不是物理上的真实，但抓住了我们的直觉，即速度应该缓慢变化).
如果我们认为估计状态应该更平滑，那么实现这一目标的一种方法是说我们对测量的信心不如我们的动态(即我们有更高的 observation_covariance，相对于我们的 observation_covariance代码>state_covariance).
从上面代码的末尾开始，将observation covariance 固定为之前估计值的 10 倍，需要将 em_vars 设置为如下所示，以避免重新估计观察协方差(请参阅
最后，如果您想在线使用此拟合过滤器，您可以使用 filter_update 方法来实现.注意这里使用的是 filter 方法而不是 smooth 方法，因为 smooth 方法只能应用于批量测量.更多
以及时间信息(在我的笔记本电脑上).
构建和训练 kf3 的时间:0.0677888393402 秒更新 kf3 的时间:0.00038480758667 秒更新 kf3 的时间:0.000465154647827 秒更新 kf3 的时间:0.000463008880615 秒[EDIT] The answer by @Claudio gives me a really good tip on how to filter out outliers. I do want to start using a Kalman filter on my data though. So I changed the example data below so that it has subtle variation noise which are not so extreme (which I see a lot as well). If anybody else could give me some direction on how to use PyKalman on my data that would be great. [/EDIT]
For a robotics project I'm trying to track a kite in the air with a camera. I'm programming in Python and I pasted some noisy location results below (every item also has a datetime object included, but I left them out for clarity).
[ # 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)}
]
I first thought of manually calculating outliers and then simply removing them from the data in real time. Then I read about Kalman filters and how they are specifically meant to smoothen out noisy data. So after some searching I found the PyKalman library which seems perfect for this. Since I was kinda lost in the whole Kalman filter terminology I read through the wiki and some other pages on Kalman filters. I get the general idea of a Kalman filter, but I'm really lost in how I should apply it to my code.
In the PyKalman docs I found the following example:
>>> 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)
I simply substituted the observations for my own observations as follows:
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)
but that doesn't give me any meaningful data. For example, the smoothed_state_means becomes the following:
>>> 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]])
Could a brighter soul than me give me some hints or examples in the right direction? All tips are welcome!
TL;DR, see the code and picture at the bottom.
I think a Kalman filter could work quite well in your application, but it will require a little more thinking about the dynamics/physics of the kite.
I would strongly recommend reading this webpage. I have no connection to, or knowledge of the author, but I spent about a day trying to get my head round Kalman filters, and this page really made it click for me.
Briefly; for a system which is linear, and which has known dynamics (i.e. if you know the state and inputs, you can predict the future state), it provides an optimal way of combining what you know about a system to estimate it's true state. The clever bit (which is taken care of by all the matrix algebra you see on pages describing it) is how it optimally combines the two pieces of information you have:
Measurements (which are subject to "measurement noise", i.e. sensors not being perfect)
Dynamics (i.e. how you believe states evolve subject to inputs, which are subject to "process noise", which is just a way of saying your model doesn't match reality perfectly).
You specify how sure you are on each of these (via the co-variances matrices R and Q respectively), and the Kalman Gain determines how much you should believe your model (i.e. your current estimate of your state), and how much you should believe your measurements.
Without further ado let's build a simple model of your kite. What I propose below is a very simple possible model. You perhaps know more about the Kite's dynamics so can create a better one.
Let's treat the kite as a particle (obviously a simplification, a real kite is an extended body, so has an orientation in 3 dimensions), which has four states which for convenience we can write in a state vector:
x = [x, x_dot, y, y_dot],
Where x and y are the positions, and the _dot's are the velocities in each of those directions. From your question I'm assuming there are two (potentially noisy) measurements, which we can write in a measurement vector:
z = [x, y],
We can write-down the measurement matrix (H discussed here, and observation_matrices in pykalman library):
z = Hx => H = [[1, 0, 0, 0], [0, 0, 1, 0]]
We then need to describe the system dynamics. Here I will assume that no external forces act, and that there is no damping on the movement of the Kite (with more knowledge you may be able to do better, this effectively treats external forces and damping as an unknown/unmodeled disturbance).
In this case the dynamics for each of our states in the current sample "k" as a function of states in the previous samples "k-1" are given as:
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)
Where "dt" is the time-step. We assume (x, y) position is updated based on current position and velocity, and velocity remains unchanged. Given that no units are given we can just say the velocity units are such that we can omit "dt" from the equations above, i.e. in units of position_units/sample_interval (I assume your measured samples are at a constant interval). We can summarise these four equations into a dynamics matrix as (F discussed here, and transition_matrices in pykalman library):
x(k) = Fx(k-1) => F = [[1, 1, 0, 0], [0, 1, 0, 0], [0, 0, 1, 1], [0, 0, 0, 1]].
We can now have a go at using the Kalman filter in python. Modified from your code:
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()
Which produced the following showing it does a reasonable job of rejecting the noise (blue is x position, red is y position, and x-axis is just sample number).
Suppose you look at the plot above and think it looks too bumpy. How could you fix that? As discussed above a Kalman filter is acting on two pieces of information:
- Measurements (in this case of two of our states, x and y)
- System dynamics (and the current estimate of state)
The dynamics captured in the model above are very simple. Taken literally they say that the positions will be updated by current velocities (in an obvious, physically reasonable way), and that velocities remain constant (this is clearly not physically true, but captures our intuition that velocities should change slowly).
If we think the estimated state should be smoother, one way to achieve this is to say we have less confidence in our measurements than our dynamics (i.e. we have a higher observation_covariance, relative to our state_covariance).
Starting from end of code above, fix the observation covariance to 10x the value estimated previously, setting em_vars as shown is required to avoid re-estimation of the observation covariance (see 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()
Which produces the plot below (measurements as dots, state estimates as dotted line). The difference is rather subtle, but hopefully you can see that it's smoother.
Finally, if you want to use this fitted filter on-line, you can do so using the filter_update method. Note that this uses the filter method rather than the smooth method, because the smooth method can only be applied to batch measurements. More 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()
Plot below shows the performance of the filter method, including 3 points found using the filter_update method. Dots are measurements, dashed line are state estimates for filter training period, solid line are states estimates for "on-line" period.
And the timing information (on my laptop).
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
这篇关于如何在 Python 中使用卡尔曼滤波器获取位置数据?的文章就介绍到这了，希望我们推荐的答案对大家有所帮助，也希望大家多多支持IT屋！
