如何在Python中使用卡爾曼濾波器來獲取位置數據？

Question

[編輯] @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庫看起來很完美。 由於我在整個卡爾曼濾波器術語中丟失了，我通過維基和卡爾曼濾波器上的其他一些頁面閱讀。 我得到了卡爾曼濾波器的一般概念，但我真的迷失了我應該如何將它應用到我的代碼中。

在PyKalman文檔中，我找到了以下示例：

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

能比我更聰明的靈魂能給我一些正確方向的暗示或例子嗎？ 歡迎所有提示！

Answer 1

TL; DR，請參閱底部的代碼和圖片。

我認為卡爾曼濾波器在你的應用中可以很好地工作，但它需要更多地思考風箏的動力學/物理學。

我強烈建議您閱讀此網頁 。 我與作者沒有任何聯系或知識，但我花了大約一天試圖讓我的頭圍繞卡爾曼過濾器，這個頁面真的讓我點擊了。

簡單地說， 對於一個線性系統，並且具有已知動力學的系統（即如果你知道狀態和輸入，你可以預測未來狀態），它提供了一種結合你所知道的系統的最佳方式來估計它的真實狀態。 聰明的位（由您在描述它的頁面上看到的所有矩陣代數處理）是它如何最佳地組合您擁有的兩條信息：

1. 測量（受“測量噪聲”影響，即傳感器不完美）

2. 動力學（即你如何相信狀態是根據輸入而發展的，這些輸入受到“過程噪聲”的影響，這只是說你的模型與現實完美匹配的一種方式）。

你可以指定你對每一個的確定程度（分別通過協方差矩陣R和Q ）， 卡爾曼增益決定你應該相信你的模型多少（即你當前對你的狀態的估計），以及你應該多少相信你的測量。

不用多說，讓我們建立一個簡單的風箏模型。 我在下面提出的是一個非常簡單的可能模型。 您可能對風箏的動力學了解得更多，因此可以創造更好的動力學。

讓我們將風箏視為一個粒子（顯然是一個簡化，一個真正的風箏是一個擴展的身體，所以有一個三維的方向），它有四個狀態，為方便起見我們可以寫一個狀態向量：

x = [x，x_dot，y，y_dot]，

其中x和y是位置，_dot是每個方向的速度。 根據您的問題，我假設有兩個（可能是噪聲的）測量值，我們可以在測量矢量中寫入：

z = [x，y]，

我們可以寫下來測量矩陣（H討論這里 ，和observation_matrices在pykalman庫）：

z = H x => 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軸只是樣本號）。

假設你看一下上面的情節並認為它看起來太顛簸了。 你怎么能解決這個問題？ 如上所述，卡爾曼濾波器作用於兩條信息：

1. 測量（在這種情況下，我們的兩個州，x和y）
2. 系統動力學（以及當前的狀態估計）

上面模型中捕獲的動力學非常簡單。 從字面上看，他們說位置將通過當前速度（以明顯的，物理上合理的方式）更新，並且速度保持不變（這顯然不是物理上的真實，但捕獲了我們的直覺，即速度應該緩慢變化）。

如果我們認為估計的狀態應該更平滑，那么實現這一點的一種方法是說我們對測量的信心不如我們的動態（即相對於我們的state_covariance我們有更高的observation_covariance state_covariance ）。

從上面的代碼結束開始，將observation covariance修正為先前估計的值的10倍，設置em_vars如圖所示是為了避免重新估計觀測協方差（參見此處 ）

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方法只能應用於批量測量。 更多信息 ：

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顯示了過濾方法的性能，包括使用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

Answer 2

從我所看到的，使用卡爾曼濾波可能不是你的情況下的正確工具。

這樣做怎么樣？ ：

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

請參閱此處了解更多信息以及我從上面獲得上述代碼的來源。