UKFの気持ち[準備①:非線形変換における期待値]

カルマンフィルタはガウシアンフィルタでもあり、確率密度関数(PDF)の更新計算をどうやって実行するのか?ということに尽きる。もし、確率密度関数がガウシアンで、線形変換しかない場合であれば、積分をする必要はなく、逐次的な計算のみでよいことから線形カルマンフィルタの逐次計算の導出がされる。では非線形変換を伴う場合はどうだろうか。
非線形カルマンフィルタに拡張カルマンフィルタ(Extended Kalman Filter:EKF)があるが、この近似の正しさは?無香料カルマンフィルタ(Unscented Kalman Filter:UKF)との差は?少しずつ書き留めていきたいと思う。

ここではある程度妥当な仮定を置きながら非線形変換を伴う期待値と共分散を計算してみます。本記事ではまずは期待値について計算します。共分散はまた次の記事にします。

以下では極座標から直交座標への変換を考えていきます。変換前の極座標$x$を

$$
\begin{align}
\boldsymbol{x}
=\begin{bmatrix} x_1 \\ x_2 \end{bmatrix}
=\begin{bmatrix} r \\ \theta \end{bmatrix}
\end{align}
$$

とする。このとき直交座標への変換は

$$
\begin{align}
\boldsymbol{y}
=\begin{bmatrix} y_1 \\ y_2 \end{bmatrix}
=\begin{bmatrix} r \cos\theta \\ r \sin\theta \end{bmatrix}
=h(\boldsymbol{x})
\end{align}
$$

となります。$r,\theta$の平均と分散を

$$
\begin{align}
E[x_1]&=E[r]=\bar{r}=1 \\
E[x_2]&=E[\theta]=\bar{\theta}=\frac{\pi}{2} \\
E[x_1^2]&=E[r^2]=\sigma_r^2 \\
E[x_2^2]&=E[\theta^2]=\sigma_\theta^2
\end{align}
$$
のように仮定します。このとき直交座標への非線形変換先での平均値をテイラー展開の一次までを使って計算してみる。

$$
\begin{align}
\bar{\boldsymbol{y}}
&=E[h(\boldsymbol{x})] \\
&=E\left[h(\boldsymbol{\bar{x}})+ \left.\frac{\partial h}{\partial x}\right|_{\boldsymbol{x}=\bar{\boldsymbol{x}}}
(\boldsymbol{x}-\bar{\boldsymbol{x}}) + \cdots\right] \\
&\sim
E\left[h(\boldsymbol{\bar{x}})+ \left.\frac{\partial h}{\partial x}\right|_{\boldsymbol{x}=\bar{\boldsymbol{x}}}
(\boldsymbol{x}-\bar{\boldsymbol{x}}) \right] \\
&=
h(\boldsymbol{\bar{x}})+ \left.\frac{\partial h}{\partial x}\right|_{\boldsymbol{x}=\bar{\boldsymbol{x}}}
E[(\boldsymbol{x}-\bar{\boldsymbol{x}}) ] \\
&=
h(\boldsymbol{\bar{x}})+ \left.\frac{\partial h}{\partial x}\right|_{\boldsymbol{x}=\bar{\boldsymbol{x}}}
(E[\boldsymbol{x}]-\bar{\boldsymbol{x}}) ) \\
&=
h(\boldsymbol{\bar{x}} ) \\
&=
\begin{bmatrix} \bar{r} \cos\bar{\theta}\\ \bar{r} \sin\bar{\theta} \end{bmatrix} \\
&=
\begin{bmatrix} 0 \\ 1\end{bmatrix}
\end{align}
$$

平均値をそのまま入れるだけという一次の線形化としては直観に合うような結論になった。さて、一次で打ち切ったわけだが果たしてこれは近似として妥当だろうか。もう少し深堀してみたいと思います。まず、$x$を平均とその偏差となるように変数を置きなおします。

$$
\begin{align}
r &= \bar{r}+\tilde{r} \\
\theta &=\bar{\theta} + \tilde{\theta}
\end{align}
$$

このとき平均

$$
\begin{align}
\bar{y}_1 &= E[ r \cos\theta] \\
&=E \left[(\bar{r}+\tilde{r}) \cos (\bar{\theta} + \tilde{\theta})\right] \\
&=E \left[(\bar{r}+\tilde{r}) (\cos\bar{\theta}\cos\tilde{\theta} – \sin\bar{\theta}\sin\tilde{\theta} )\right] \\
&=\bar{r}\cos\bar{\theta} E[\cos\tilde{\theta}]-\bar{r}\sin\bar{\theta} E[\sin\tilde{\theta}]
+\cos\bar{\theta}E[\tilde{r} \cos\tilde{\theta}] – \sin\bar{\theta}E[\tilde{r} \sin\tilde{\theta}] \\
\end{align}
$$

$r$と$\theta$は独立であると仮定すると

$$
\begin{align}
\bar{y}_1
&=\bar{r}\cos\bar{\theta} E[\cos\tilde{\theta}]-\bar{r}\sin\bar{\theta} E[\sin\tilde{\theta}]
+\cos\bar{\theta}E[\tilde{r}]E[ \cos\tilde{\theta}] – \sin\bar{\theta}E[\tilde{r}]E[ \sin\tilde{\theta}] \\
\end{align}
$$

平均の値を思い出して代入すると

$$
\begin{align}
\bar{y}_1 = 0
\end{align}
$$

$\bar{y}_1$については特に近似なしで$0$になることがわかった。一次で打ち切った結果と同じで安心ですね。次に、$\bar{y}_2$を考えてみます。

$$
\begin{align}
\bar{y}_2 &= E[r \sin\theta] \\
&=E \left[(\bar{r}+\tilde{r}) \sin (\bar{\theta} + \tilde{\theta})\right] \\
&=E \left[(\bar{r}+\tilde{r}) ( \sin \bar{\theta} \cos\tilde{\theta} + \cos\bar{\theta} \sin\tilde{\theta} )\right] \\
&= \bar{r}\sin\bar{\theta}E[\cos\tilde{\theta}] + \bar{r}\cos\bar{\theta}E[\sin\tilde{\theta}] + \sin\bar{\theta}E[\tilde{r}\cos\tilde{\theta}] + \cos\bar{\theta} E[\tilde{r}\sin\tilde{\theta} ]
\end{align}
$$
先ほどと同様に$r$と$\theta$独立と仮定すると

$$\begin{align}
\bar{y}_2
&= \bar{r}\sin\bar{\theta}E[\cos\tilde{\theta}] + \bar{r}\cos\bar{\theta}E[\sin\tilde{\theta}] + \sin\bar{\theta}E[\tilde{r}]E[\cos\tilde{\theta}] + \cos\bar{\theta} E[\tilde{r}]E[\sin\tilde{\theta} ] \\
&= \bar{r}\sin\bar{\theta}E[\cos\tilde{\theta}] \\
&=E[\cos\tilde{\theta}] \\
&=\int_{-\infty}^{\infty} \cos\tilde{\theta} p(\tilde{\theta}) d\tilde{\theta}
\end{align}$$

確率密度関数が原点対称で$-\theta_m \sim \theta_m$で一様であると仮定すると

$$\begin{align}
\bar{y}_2
&=\int_{-\theta_m}^{\theta_m} \cos\tilde{\theta} \frac{1}{2\theta_m} d\tilde{\theta} \\
&=\frac{\sin\theta_m}{\theta_m} \leq 1
\end{align}$$

$\theta_m>0$のときには1よりに小さい値になることがわかった。PDFが一様と仮定したときの$\theta_m$次第ではあるが、1より小さい値になってしまい$y_2\sim 1$とするのは場合によっては良くなさそう。