概要

Unscented Kalman Filterとは非線形モデルに対応するカルマンフィルターの1つである。非線形関数の微分を計算しないため「微分なしフィルター」とも呼ばれる。前周期の状態の推定結果に基づいて、有限個の状態を分布の代表点として抽出し（これをシグマ点と呼ぶ）、それぞれの点に対して予測や観測に対応する非線形変換を行う。最終的にそれらを重み付けして統合することで次の周期の期待値や分散共分散行列を求める。期待値や分さん共分散行列を求める時にシグマ点をどのような重みで使うか（重み係数）がハイパーパラメータとなる。

シグマ点および重みの決定

$\vec{x}=(x_{1}, x_{2}, ...., x_{n})^{T}$ に対してその期待値 $\vec{\mu}$ と分散共分散行列 $\Sigma$ が求まっている前提で、シグマ点は以下のように選ばれる。

$\begin{eqnarray} \vec{\chi}^{0} &=& \vec{\mu} \\\ \vec{\chi}^{i} &=& \vec{\mu} + \left(\sqrt{(n+\lambda(\alpha, \kappa))\Sigma} \right)_{i} \ \ (i=1, 2, ...n) \\\ \vec{\chi}^{i+n} &=& \vec{\mu} - \left(\sqrt{(n+\lambda(\alpha, \kappa))\Sigma} \right)_{i} \ \ (i=1, 2, ...n) \\\ \lambda(\alpha, \kappa) &\equiv& \alpha^{2}(n+\kappa) - n \end{eqnarray}$

$\alpha$ と $\kappa$ が重みに関するハイパーパラメータである。ここで $(\sqrt{A})_{i}$ は行列 $A$ の各成分の平方根を取った行列の $i$ 列目を取ったベクトルを表す。すなわち $n$ 次元の状態量に対しては $2n+1$ 点のシグマ点を選ぶ。

シグマ点の例

重み

期待値や分散共分散行列を推定する時に、各シグマ点をどの程度の重みで使用するかを決める必要がある。各シグマ点 $i$ に対して、平均と共分散を算出するときの重みをそれぞれ $w_{m}^{i}$ 、 $w_{c}^{i}$ とすると、重みは自由パラメータ $\alpha, \beta, \kappa$ を用いて

$\begin{eqnarray} w_{m}^{0}(\alpha, \kappa) &=& \frac{\lambda(\alpha, \kappa)}{n+\lambda(\alpha, \kappa)} \\\ w_{c}^{0}(\alpha, \beta, \kappa) &=& \frac{\lambda(\alpha, \kappa)}{n+\lambda(\alpha, \kappa)} + (1-\alpha^{2}+\beta) \\\ w_{m}^{i}(\alpha, \kappa) = w_{c}^{i}(\alpha, \kappa) &=& \frac{1}{2(n+\lambda(\alpha, \kappa))} \ \ (i=1, 2,... 2n) \end{eqnarray}$

と表される。 $\alpha, \kappa$ は期待値からどれだけ外れたシグマ点を選択するかを決めるスケールパラメータ、 $\beta$ は共分散を求めるときのゼロ点（期待値のシグマ点）に対する重みを決めるパラメータであり、ガウス分布の場合は $\beta=2$ であることが知られている。

アルゴリズムまとめ

Unscented Kalman Filterのアルゴリズムをまとめる。線形カルマンフィルターと同様に、 $\vec{x}_{t-1}^{est}$ 、 $\vec{x}_{t}^{obs}$ 、 $\vec{u}_{t}$ 、 $V(\vec{x}_{t-1}^{est})$ を入力として $\vec{x}_{t}^{est}$ と $V(\vec{x}_{t}^{est})$ を求めることを目指す。事前に自由パラメータ $\alpha, \beta, \kappa$ を決めておく。シグマ点の作成を予測時と観測時の2回実施することに注意。

1: $\vec{x}_{t-1}^{est}$ 、 $V(\vec{x}_{t-1}^{est})$ に基づいてシグマ点 $\chi_{t-1} \equiv (\vec{\chi}_{t-1}^{0}, \vec{\chi}_{t-1}^{1}, ..., \vec{\chi}_{t-1}^{2n})$ を決める。
2: 予測モデル $g$ （すなわち $\vec{x}_{t}^{true} = g(\vec{x}_{t-1}^{true})$ となる $g$ ）により各シグマ点の予測点を算出する。

$\begin{equation} \chi_{t}^{odom} \equiv g(\chi_{t-1}) \equiv \left(g(\vec{\chi}_{t-1}^{0}), g(\vec{\chi}_{t-1}^{1}), ... g(\vec{\chi}_{t-1}^{2n})\right) \end{equation}$

3: シグマ点を重みづけして、予測値の期待値と分散共分散行列を更新する。線形カルマンフィルターで述べたノイズ $\vec{w}_{t}$ の分散共分散行列も入れることに注意。

$\begin{eqnarray} \vec{x}_{t}^{odom} &=& \sum_{i} w_{m}^{i} g(\vec{\chi}_{t-1}^{i}) \\\ V(\vec{x}_{t}^{odom}) &=& \sum_{i} w_{c}^{i} \left( g(\vec{\chi}_{t-1}^{i}) - \vec{x}_{t}^{odom} \right) \left( g(\vec{\chi}_{t-1}^{i}) - \vec{x}_{t}^{odom} \right)^{T} + V(\vec{w}) \end{eqnarray}$

4: $\vec{x}_{t}^{odom}$ 、 $V(\vec{x}_{t}^{odom})$ に基づいてシグマ点 $Z_{t} \equiv (\vec{Z}_{t}^{0}, \vec{Z}_{t}^{1}, ..., \vec{Z}_{t}^{2n})$ を決める。
5: 観測モデル $h$ （すなわち $\vec{z}_{t}=h(\vec{x}_{t}^{true})$ となる $h$ ）により各シグマ点を観測点に変換する。

$\begin{equation} h(Z_{t}) \equiv \left(h(\vec{Z}_{t}^{0}), h(\vec{Z}_{t}^{1}), ... h(\vec{Z}_{t}^{2n})\right) \end{equation}$

6: 観測点の期待値と分散共分散行列を更新する。線形カルマンフィルターで述べた観測誤差 $\vec{v}_{t}$ の分散共分散行列も入れることに注意。

$\begin{eqnarray} \vec{\hat{z}}_{t} &=& \sum_{i} w_{m}^{i} h(\vec{Z}_{t}^{i}) \\\ V(\vec{\hat{z}}_{t}) &=& \sum_{i} w_{c}^{i} \left( h(\vec{Z}_{t}^{i}) - \vec{\hat{z}}_{t} \right) \left( h(\vec{Z}_{t}^{i}) - \vec{\hat{z}}_{t} \right)^{T} + V(\vec{v}) \end{eqnarray}$

ここで出てくる $\hat{z}_{t}$ は「予測ステップの位置にいたとした場合の観測結果」を表しており、実際の観測結果 $z_{t}$ とは異なるものであることに注意。
7: 予測点と観測点の分散共分散行列 $\Sigma_{t}^{xz}$ をもとめる。

$\begin{equation} \Sigma_{t}^{xz} = \sum_{i} w_{c}^{i} (\vec{Z}^{i}_{t} - \vec{x}_{t}^{odom}) \left( h(\vec{Z}_{t}^{i}) - \vec{\hat{z}}_{t} \right)^\mathsf{T} \end{equation}$

ここでsummationしているコンポーネントは、 $i$ 番目の各シグマ点について「予測ステップ後の平均からのずれ」と「予想される観測値と実際の観測値のずれ」を掛け合わせたものとして表されていることがわかる。 8: 最適カルマンゲインを求める。
ここで線形カルマンフィルターの最適カルマンゲインを思い出すと

$\begin{equation} K_{t}^{opt}=V(\vec{x}_{t}^{odom}) H_{t}^{T} \left( H_{t} V( \vec{x}_{t}^{odom}) H_{t}^{T} + V ( \vec{v}) ) \right)^{-1} \end{equation}$

であったが、これを非線形関数版に対応させる。

$\begin{eqnarray} V(\vec{x}_{t}^{odom}) H_{t}^{T} &=& E\left( (\vec{x}_{t}^{odom} - \vec{x}_{t}^{true}) (\vec{x}_{t}^{odom} - \vec{x}_{t}^{true})^{T} \right) H_{t}^{t} \\\ &=& E\left( (\vec{x}_{t}^{odom} - \vec{x}_{t}^{true}) (H_{t}\vec{x}_{t}^{odom} - H_{t}\vec{x}_{t}^{true})^{T} \right) \\\ &\rightarrow& E\left( (\vec{x}_{t}^{odom} - \vec{x}_{t}^{true}) (h(\vec{x}_{t}^{odom}) - h(\vec{x}_{t}^{true})^{T} \right) \\\ &\rightarrow& \Sigma_{t}^{xz} \\\ H_{t} V( \vec{x}_{t}^{odom}) H_{t}^{T} + V ( \vec{v}) &=& E\left( H_{t}(\vec{x}_{t}^{odom}-\vec{x}_{t}^{true})(H_{t}(\vec{x}_{t}^{odom}-\vec{x}_{t}^{true}))^{T} \right) + V ( \vec{v}) \\\ &\rightarrow& E\left( (h(\vec{x}_{t}^{odom}) - h(\vec{x}_{t}^{true})) (h(\vec{x}_{t}^{odom}) - h(\vec{x}_{t}^{true}))^{T} \right) \\\ &\rightarrow& V(\vec{z}_{t}) \end{eqnarray}$

したがって、Unscented Kalman Filterの最適カルマンゲインは

$\begin{equation} K_{t}^{opt} = \Sigma_{t}^{xz} V(\vec{z}_{t})^{-1} \end{equation}$

9: 推定値の分散共分散行列の更新
線形カルマンフィルターでは

$\begin{equation} \vec{x}_{t}^{est} = \vec{x}_{t}^{odom} + K_{t}^{opt}(\vec{z}_{t} - H_{t}\vec{x}_{t}^{odom}) \end{equation}$

だったので、Unscented Kalman Filterの場合

$\begin{equation} \vec{x}_{t}^{est} = \vec{x}_{t}^{odom} + K_{t}^{opt}(\vec{z}_{t} - \vec{\hat{z}}_{t}) \end{equation}$

$\begin{equation} V(\vec{x}_{t}^{est}) = V(\vec{x}_{t}^{odom}) + K_{t}^{opt} V(\vec{z}_{t}) (K_{t}^{opt})^\mathsf{T} \end{equation}$

参考

確率ロボティクス (ROBOT books) | Sebastian Thrun, Wolfram Burgard, Dieter Fox, 上田隆一 |本 | 通販 | Amazon