前回 : カルマンフィルタの基礎第2回 - ma-hi-ro Hobby

カルマンフィルタの基礎第3回

第1回では、「真値を一次関数でモデル化して、最小二乗法で推定する」を行った
今回は、真値の確率分布が正規分布（ガウス分布）になる、と仮定して、
ベイズの定理を使って、真値を推定する方法を紹介する

カルマンフィルタの基礎第3回

ベイズの定理

カルマンフィルタでは、観測値として $y$ が計測された際に、真値 $x$ の値を推定する
これは、ベイズの定理で言うところの「条件付き確率」になり、
$p(x \mid y)$ と表現できる

ベイズの定理の導出

ベイズの定理を導き出す上では、 $x$ と $y$ の同時確率を考える
$x$ と $y$ の同時確率は
「 $x$ が発生する確率」と「 $x$ のときに $y$ が発生する確率」の掛け算と、
「 $y$ が発生する確率」と「 $y$ のときに $x$ が発生する確率」の掛け算と、
に一致する

$\displaystyle P(x \land y) = p(x) p(y \mid x) = p(y) p(x \mid y)$

この関係から、下記のベイズの定理を導き出せる

$\displaystyle p(x \mid y) = \frac{p(x) p(y \mid x)}{p(y)}$

正規分布の最尤推定法（スカラの場合）

事前に、 $x$ について、 $\bar{x}$ と $\sigma_x^2$ が分かっているとすると、
「 $x$ が発生する確率」は、下記になる

$\displaystyle p_1 (x) = \frac{1}{\sqrt{2 \pi \sigma_x^2}} \exp \left\{ - \frac{ (x - \bar{x})^2}{2 \sigma_x^2} \right\}$

次に、「 $x$ のときに $y$ が発生する確率」は、
$x$ と $y$ の関係 $y = c x + \omega$ から求めることができる

こちらも事前に、 $\omega$ について、 $\bar{\omega}$ と $\sigma_\omega^2$ が分かっているとすると、
「 $x$ のときに $y$ が発生する確率」は、「 $\omega$ が $y - c x$ になる確率」になるので、下記になる

$\displaystyle \begin{aligned} p_2 (y \mid x) &= \frac{1}{\sqrt{2 \pi \sigma_\omega^2}} \exp \left\{ - \frac{ (\omega - \bar{\omega})^2}{2 \sigma_\omega^2} \right\} \\ &= \frac{1}{\sqrt{2 \pi \sigma_\omega^2}} \exp \left\{ - \frac{ [(y - \bar{y}) - c (x - \bar{x})]^2}{2 \sigma_\omega^2} \right\} \end{aligned}$

ここで、 $\bar{\omega} = \bar{y} - c \bar{x}$ の関係も使用した

最後に、「 $y$ が発生する確率」を求めると、

$\displaystyle p_3 (y) = \frac{1}{\sqrt{2 \pi \sigma_y^2}} \exp \left\{ - \frac{ (y - \bar{y})^2}{2 \sigma_y^2} \right\}$

となるが、この $\sigma_y^2$ は、 $y = cx + \omega$ の関係から求めることができる

$\displaystyle \begin{aligned} \sigma_y^2 &= E \left[ (y - \bar{y})^2 \right] \\ &= E \left[ \{ c (x - \bar{x}) + (\omega - \bar{\omega}) \}^2 \right] \\ &= c^2 E \left[ (x - \bar{x})^2 \right] + 2 c E \left[ (x - \bar{x}) (\omega - \bar{\omega}) \right] + E \left[ (\omega - \bar{\omega})^2 \right] \\ &= c^2 \sigma_x^2 + \sigma_\omega^2 \\ \end{aligned}$

ここで、 $E \left[ (x - \bar{x}) (\omega - \bar{\omega}) \right] = 0$ を用いた

これらから、 $p(x \mid y)$ を計算すると

$\displaystyle \begin{aligned} p(x \mid y) &= \frac{p_1(x) p_2(y \mid x)}{p_3(y)} \\ &= \frac{\sqrt{2 \pi \sigma_y^2}}{\sqrt{2 \pi \sigma_x^2} \sqrt{2 \pi \sigma_\omega^2}} \exp \left\{ - \frac{ (x - \bar{x})^2}{2 \sigma_x^2} - \frac{ [(y - \bar{y}) - c (x - \bar{x})]^2}{2 \sigma_\omega^2} + \frac{ (y - \bar{y})^2}{2 \sigma_y^2} \right\} \\ \end{aligned}$

$x$ の平方完成

指数部を $x$ について整理する
$x$ について平方完成すれば、最も確率の高くなる $x$ の値が分かり、推定値 $\hat{x}$ を求めることができる

まずは、指数部の1,2項を抽出する( $-1/2$ も取り除く)
さらに、 $x' = x - \bar{x}$ , $y' = x - \bar{y}$ とおくと

$\displaystyle \begin{aligned} \frac{ x'^2 }{\sigma_x^2} + \frac{(y' - c x')^2}{\sigma_\omega^2} &= \frac{ \sigma_\omega^2 x'^2 + \sigma_x^2 (y'^2 - 2 c y' x' + c^2 x'^2) }{\sigma_x^2 \sigma_\omega^2} \\ (\sigma_x^2 \sigma_\omega^2) (\frac{ x'^2 }{\sigma_x^2} + \frac{(y' - c x')^2}{\sigma_\omega^2}) &= (c^2 \sigma_x^2 + \sigma_\omega^2) x'^2 - 2 c \sigma_x^2 y' x' + \sigma_x^2 y'^2 \\ &= (c^2 \sigma_x^2 + \sigma_\omega^2) (x' - \frac{c \sigma_x^2 y'}{c^2 \sigma_x^2 + \sigma_\omega^2})^2 - \frac{c^2 \sigma_x^4 y'^2}{c^2 \sigma_x^2 + \sigma_\omega^2} + \sigma_x^2 y'^2 \\ &= (c^2 \sigma_x^2 + \sigma_\omega^2) (x' - \frac{c \sigma_x^2 y'}{c^2 \sigma_x^2 + \sigma_\omega^2})^2 - \frac{c^2 \sigma_x^4 y'^2}{c^2 \sigma_x^2 + \sigma_\omega^2} + \sigma_x^2 y'^2 \frac{c^2 \sigma_x^2 + \sigma_\omega^2}{c^2 \sigma_x^2 + \sigma_\omega^2} \\ &= (c^2 \sigma_x^2 + \sigma_\omega^2) (x' - \frac{c \sigma_x^2 y'}{c^2 \sigma_x^2 + \sigma_\omega^2})^2 - \frac{c^2 \sigma_x^4 y'^2}{c^2 \sigma_x^2 + \sigma_\omega^2} + \frac{c^2 \sigma_x^4 y'^2 + \sigma_x^2 \sigma_\omega^2 y'^2}{c^2 \sigma_x^2 + \sigma_\omega^2} \\ &= (c^2 \sigma_x^2 + \sigma_\omega^2) (x' - \frac{c \sigma_x^2 y'}{c^2 \sigma_x^2 + \sigma_\omega^2})^2 + \frac{\sigma_x^2 \sigma_\omega^2 y'^2}{c^2 \sigma_x^2 + \sigma_\omega^2} \\ \frac{ x'^2 }{\sigma_x^2} + \frac{(y' - c x')^2}{\sigma_\omega^2} &= \frac{c^2 \sigma_x^2 + \sigma_\omega^2}{\sigma_x^2 \sigma_\omega^2} (x' - \frac{c \sigma_x^2 y'}{c^2 \sigma_x^2 + \sigma_\omega^2})^2 + \frac{y'^2}{c^2 \sigma_x^2 + \sigma_\omega^2} \\ \end{aligned}$

ここに、3項目を追加すると、指数部は下記になる

$\displaystyle \begin{aligned} \frac{ x'^2 }{\sigma_x^2} + \frac{(y' - c x')^2}{\sigma_\omega^2} - \frac{ y'^2 }{\sigma_y^2} &= \frac{c^2 \sigma_x^2 + \sigma_\omega^2}{\sigma_x^2 \sigma_\omega^2} (x' - \frac{c \sigma_x^2 y'}{c^2 \sigma_x^2 + \sigma_\omega^2})^2 + \frac{y'^2}{c^2 \sigma_x^2 + \sigma_\omega^2} - \frac{ y'^2 }{c^2 \sigma_x^2 + \sigma_\omega^2} \\ &= \frac{c^2 \sigma_x^2 + \sigma_\omega^2}{\sigma_x^2 \sigma_\omega^2} (x' - \frac{c \sigma_x^2 y'}{c^2 \sigma_x^2 + \sigma_\omega^2})^2 \\ \end{aligned}$

次に係数部を計算すると下記になる

$\displaystyle \begin{aligned} \frac{\sqrt{2 \pi \sigma_y^2}}{\sqrt{2 \pi \sigma_x^2} \sqrt{2 \pi \sigma_\omega^2}} &= \sqrt{\frac{c^2 \sigma_x^2 + \sigma_\omega^2}{2 \pi \sigma_x^2 \sigma_\omega^2}} \end{aligned}$

ここで、

$\displaystyle \sigma^2 = \frac{\sigma_x^2 \sigma_\omega^2}{c^2 \sigma_x^2 + \sigma_\omega^2} = (c^2 \sigma_\omega^{-2} + \sigma_x^{-2})^{-1}$

とおくと、

$\displaystyle \begin{aligned} p(x \mid y) &= \frac{1}{\sqrt{2 \pi \sigma^2}} \exp \left\{ - \frac{ (x' - c \sigma^2 \sigma_\omega^{-2} y')^2 }{2 \sigma^2} \right\} \\ \end{aligned}$

さらに、 $\hat{x}$ を

$\displaystyle \hat{x} = \bar{x} + c \sigma^2 \sigma_\omega^{-2} \{y - (c \bar{x} + \bar{\omega}) \}$

とおくと、

$\displaystyle \begin{aligned} p(x \mid y) &= \frac{1}{\sqrt{2 \pi \sigma^2}} \exp \left\{ - \frac{ (x - \hat{x})^2 }{2 \sigma^2} \right\} \\ \end{aligned}$