位置データにPythonでカルマンフィルターを使用するには？

Question

[編集] @Claudioの回答は、外れ値を除外する方法に関する非常に良いヒントを与えてくれます。ただし、データに対してカルマンフィルターの使用を開始します。そこで、以下のサンプルデータを変更して、それほど極端ではない微妙な変動ノイズが発生するようにしました（これも多く見られます）。他の誰かが私のデータでPyKalmanを使用する方法について何らかの方向性を与えてくれたら、それは素晴らしいことです。 [/編集]

ロボットプロジェクトの場合、空中のcameraをカメラで追跡しようとしています。私はPythonでプログラミングしています。以下にノイズの多い場所の結果を貼り付けました（すべてのアイテムにはdatetimeオブジェクトも含まれていますが、明確にするために省略しました）。

[           # 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ライブラリ を見つけました。カルマンフィルターの用語全体でちょっと迷ったので、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]])

私より明るい魂が正しい方向にいくつかのヒントや例を与えることができますか？すべてのヒントを歓迎します！

Answer 1

TL; DR、下部のコードと図を参照してください。

カルマンフィルターはアプリケーションで非常にうまく機能すると思いますが、カイトのダイナミクス/物理学についてもう少し考える必要があります。

このWebページ を読むことを強くお勧めします。筆者とは関係も知識もありませんが、カルマンフィルターを一周するために約1日を費やしました。このページは本当にクリックしてくれました。

簡単に線形で、既知のダイナミクスを持つシステム（つまり、状態と入力がわかっている場合、将来の状態を予測できます）については、システムについて知っていることを組み合わせてその真の状態を推定する最適な方法を提供します巧妙なビット（それを説明するページに表示されるすべての行列代数によって処理されます）は、2つの情報を最適に組み合わせる方法です。

1. 測定（「測定ノイズ」の影響を受ける、つまりセンサーが完全ではない）

2. ダイナミクス（つまり、「プロセスノイズ」の影響を受ける入力に応じて状態が進化することを信じる方法。これは、モデルが現実と完全に一致しないことを示す方法です）。

共分散行列[〜＃〜] r [〜＃〜]および[〜＃〜] q [〜＃〜]）、およびカルマンゲインモデル（つまり、現在の状態の推定値）、および測定値をどれだけ信じるべきかを信じてください。

苦労せずに、カイトの簡単なモデルを作成しましょう。以下に提案するのは、非常に単純な可能なモデルです。あなたはおそらくカイトのダイナミクスについてもっとよく知っているので、より良いカイトを作成できます。

カイトを粒子として扱いましょう（明らかに単純化、実際のカイトは拡張されたボディであるため、3次元の方向を持ちます）。4つの状態があり、便宜上、状態ベクトルに書き込むことができます。

x= [x、x_dot、y、y_dot]、

ここで、xとyは位置であり、_dotはそれらの各方向の速度です。あなたの質問から、2つの（潜在的にノイズの多い）測定があると仮定しています。測定ベクトルに書き込むことができます。

z= [x、y]、

測定行列（[〜＃〜] h [〜＃〜]を書き留めることができます here 、およびobservation_matricespykalmanライブラリ内）：

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として、これら4つの方程式をダイナミクスマトリックスにまとめることができます。 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軸は単なるサンプル番号です）。

上記のプロットを見て、でこぼこしすぎていると思います。どうすれば修正できますか？前述のように、カルマンフィルターは2つの情報に作用します。

1. 測定（この場合、2つの状態xとyの）
2. システムダイナミクス（および現在の状態の推定）

上記のモデルでキャプチャされたダイナミクスは非常に単純です。文字通り、彼らは現在の速度によって位置が更新されると（明らかに、物理的に合理的な方法で）、速度は一定のままであると言います（これは明らかに物理的に真実ではありませんが、速度はゆっくり変化するべきであるという直感を捕らえます）。

推定された状態をより滑らかにする必要があると考える場合、これを達成する1つの方法は、ダイナミクスよりも測定の信頼性が低いと言うことです（つまり、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メソッドを使用することに注意してください。詳細 ここ ：

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

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

here を参照し、上記のコードを入手したソースを確認してください。