Multiple Object Trackingで使うカルマンフィルタの解説

はじめに
カルマンフィルタとは
MOTで使われる状態ベクトル
共分散行列も必要
各ステップの解説
- 予測（Prediction）ステップ
- 更新（Update）ステップ
マハラノビス距離
まとめ

はじめに

R&Dユニット3年目の葉山です。

以前MOT（Multiple Object Tracking）の紹介記事をこちらのブログで公開しましたが、

tech-blog.optim.co.jp

今回はTracking-by-Detectionパラダイム*1なMOTでよく使われるカルマンフィルタの解説記事になります。高パフォーマンスな手法として最近人気のByteTrackでもカルマンフィルタが使われており、MOTとは切っても切り離せない内容となっています。

カルマンフィルタ自体奥が深い話ですので、本記事ではMOTではカルマンフィルタがどのような計算をしているのかに着目し解説を行います。また、FairMOTの実装を引用しつつ、アルゴリズムと実際の実装の対応も確認します。FairMOTはByteTrackと同著者が開発した先行手法で、その他のMOT手法でも参考にされている重要な手法です。

カルマンフィルタとは

カルマンフィルタは誤差を含む観測値から、時間変化する量を推定する際に用いられる手法の一つです。

例えば、移動している物体の位置をセンサを使って観測したとして、その観測値には誤差が含まれています。各時刻でセンサを使って位置を観測しておけば、それを使って位置や速度などを推定することができます。そして、新しい観測値と予測値の二つを使うことで正確な位置の推定ができます。

MOTで言えば、「誤差を含む観測値」というのは物体検出の結果であるbbox*2になります。また、追跡中の物体に対して、以前のフレームから求めた状態ベクトル（位置など）から新しいフレームの状態ベクトルを推定することができます。そして、「観測値であるbbox」と「予測値である状態ベクトル」の二つを用いて、新しいフレームの状態ベクトルを推定します。前者が予測のステップ、後者が更新のステップになります。この新しい状態ベクトル（に含まれる位置）が、新しいフレームでの追跡物体の位置として出力されることになります。

MOTで使われる状態ベクトル

状態ベクトルの取り方は手法によって違います。

近年のMOTの基礎となったSORTでは以下の7要素のベクトルを状態ベクトルとしています。

$\displaystyle \mathbf{x} = (x_c, y_c, s, a, \dot{x_c}, \dot{y_c}, \dot{s})\\$

ここで $(x_c, y_c)$ は画面上での物体の中心位置で、 $s$ はスケール、 $a$ はアスペクト比です（本記事では時間微分をドットで表すものとします）。

最近のMOT手法ではSORTの状態ベクトルから改良され、以下の8要素のベクトルが使用されるようになりました。

$\displaystyle \mathbf{x} = (x_c, y_c, a, h, \dot{x_c}, \dot{y_c}, \dot{a}, \dot{h})\\$

$a$ は変わらずアスペクト比ですが、 $h$ は物体の画面上での高さ(height)になります*3。FairMOTでもこの状態ベクトルを採用しています。*4

共分散行列も必要

線形カルマンフィルタでは状態がどの時刻でも正規分布に従っていることが仮定されます。そして、正規分布では平均と共分散行列で確率密度関数を一意に決めることができるので、平均と共分散行列を逐次的に更新しておけば良いことになります。共分散行列は $\mathbf{P}$ で表します。

各ステップの解説

予測（Prediction）ステップ

まずは前のフレームの状態ベクトルから、新しいフレームの状態ベクトルを予測します。

他変量のカルマンフィルタでは以下の式により予測を行います。*5

$\displaystyle \mathbf{\overline{x}} = \mathbf{F}\mathbf{x}\\ \mathbf{\overline{P}} = \mathbf{F}\mathbf{P}\mathbf{F^T} + \mathbf{Q}$

オーバーラインの記号がついたものは、このステップでの予測値です。状態ベクトルの方（ $\mathbf{\overline{x}}$ ）は事前推定値、共分散行列の方（ $\mathbf{\overline{P}}$ ）は事前誤差共分散行列と呼ばれます。

FairMOTでは以下の部分でこの計算をしています。

mean = np.dot(mean, self._motion_mat.T)
covariance = np.linalg.multi_dot((
    self._motion_mat, covariance, self._motion_mat.T)) + motion_cov

https://github.com/ifzhang/FairMOT/blob/4aa62976bde6266cbafd0509e24c3d98a7d0899f/src/lib/tracking_utils/kalman_filter.py#L119-L121

$\mathbf{Q}$ はノイズ項です（コード中のmotion_cov）。

$\mathbf{F}$ は状態遷移関数であり、前の状態から次の状態へ遷移する時間変化の関数を表します。MOTでの $\mathbf{F}$ はどうなるでしょうか？

MOTでのカルマンフィルタは等速度モデルで考えるため*6、 ${\Delta}t$ の時間ステップで以下のような更新式が成り立つでしょう。

$\displaystyle \overline{x} = x + {\Delta}t\dot{x}\\ \overline{y} = x + {\Delta}t\dot{y}\\ \overline{a} = x + {\Delta}t\dot{a}\\ \overline{h} = x + {\Delta}t\dot{h}\\ \overline{\dot{x}} = \dot{x}\\ \overline{\dot{y}} = \dot{y}\\ \overline{\dot{a}} = \dot{a}\\ \overline{\dot{h}} = \dot{h}\\$

従って、きちんと状態遷移関数を書けば以下のようになります。

$\displaystyle \mathbf{F} = \begin{pmatrix} 1&0&0&0&{\Delta}t&0&0&0\\ 0&1&0&0&0&{\Delta}t&0&0\\ 0&0&1&0&0&0&{\Delta}t&0\\ 0&0&0&1&0&0&0&{\Delta}t\\ 0&0&0&0&1&0&0&0\\ 0&0&0&0&0&1&0&0\\ 0&0&0&0&0&0&1&0\\ 0&0&0&0&0&0&0&1 \end{pmatrix}$

FairMOTでは以下の部分で定義されています。

self._motion_mat = np.eye(2 * ndim, 2 * ndim)
for i in range(ndim):
    self._motion_mat[i, ndim + i] = dt

https://github.com/ifzhang/FairMOT/blob/4aa62976bde6266cbafd0509e24c3d98a7d0899f/src/lib/tracking_utils/kalman_filter.py#L43-L45

更新（Update）ステップ

物体検出で求めたbboxを観測値として、新しいフレームでの状態ベクトルの推定を行います。

他変量のカルマンフィルタでは以下の式により更新を行います。

$\displaystyle \mathbf{y} = \mathbf{z} - \mathbf{H}\overline{\mathbf{x}}\\ \mathbf{x} = \overline{\mathbf{x}} + \mathbf{K}\mathbf{y}\\ \mathbf{P} = (\overline{\mathbf{I}} - \mathbf{K}\mathbf{H})\overline{\mathbf{P}}$

$\mathbf{z}$ は観測値であり、ここではbboxの $(x,y,a,h)$ です。 $\mathbf{y}$ は残差と呼ばれます。残差とカルマンゲイン $\mathbf{K}$ を使って更新を行います。

$\mathbf{H}$ は観測関数と呼ばれ、事前推定値を観測値と同じ空間に変換する関数です。MOTでは以下のような行列で良いでしょう。これにより状態変数の $(x,y,a,h)$ 部分だけ取り出します。

$\displaystyle \mathbf{H} = \begin{pmatrix} 1&0&0&0&0&0&0&0\\ 0&1&0&0&0&0&0&0\\ 0&0&1&0&0&0&0&0\\ 0&0&0&1&0&0&0&0\\ \end{pmatrix}$

FairMOTでは以下の部分で定義されています。

self._update_mat = np.eye(ndim, 2 * ndim)

https://github.com/ifzhang/FairMOT/blob/4aa62976bde6266cbafd0509e24c3d98a7d0899f/src/lib/tracking_utils/kalman_filter.py#L46

カルマンゲイン $\mathbf{K}$ は共分散行列と観測関数を使って計算することができますが、実際のアルゴリズムではコレスキー分解を活用して効率的に求めています。

マハラノビス距離

カルマンフィルタを実行する際、観測値の外れ値を含めて更新を行なってしまうとフィルタが不安定になり、最悪発散する場合があります。そこで、観測値のマハラノビス距離を計算し、そのカイ二乗検定により外れ値を閾値処理します。

観測値を $\mathbf{z}$ とし、平均を $\mathbf{x}$ 、共分散行列を $\mathbf{P}$ として、マハラノビス距離 $d$ は以下の式で表されます。

$\displaystyle d = (\mathbf{z} - \mathbf{x})^T\mathbf{P^{-1}}(\mathbf{z} - \mathbf{x})$

共分散行列を以下のように下三角行列にコレスキー分解すると、

$\displaystyle \mathbf{P} = \mathbf{L}\mathbf{L^T}$

逆行列は

$\displaystyle \mathbf{P^{-1}} = (\mathbf{L^{-1}})^T(\mathbf{L^{-1}})$

となるので、以下のように式変形できます。

$\displaystyle (\mathbf{z} - \mathbf{x})^T\mathbf{P^{-1}}(\mathbf{z} - \mathbf{x})\\ = (\mathbf{z} - \mathbf{x})^T(\mathbf{L^{-1}})^T(\mathbf{L^{-1}})(\mathbf{z} - \mathbf{x})\\ = (\mathbf{L^{-1}}(\mathbf{z} - \mathbf{x}))^T(\mathbf{L^{-1}}(\mathbf{z} - \mathbf{x}))\\$