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

非常に良い本です
カルマンフィルタについて、基礎をわかりやすくまとめてくれています

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

最小二乗推定法

カルマンフィルタでは、システムノイズや観測ノイズが正規分布であることを仮定して、フィルタ処理（状態の推定）を行っている
観測値の値から確率的に真値を推定する上で、ノイズが正規分布であるという仮定で、計算が非常に簡単になっている

第1回では、観測値から真値の推定方法の例として、最小二乗推定法を紹介する

推定モデルを一次関数で限定して、観測値と推定モデルの差分の二乗和が最小となるパラメータを求める

一般的には、多数のデータ群に対して、パラメータを求めるのに使う印象だが、
今回は、制御のために、「新しく観測値が追加された場合」に推定値がどうなるか、を求める

スカラの場合

まずは、観測方程式を下記のように定める
$\omega$ が観測ノイズになる

$\displaystyle y = c x + \omega$

$x$ については、下記のように平均と分散が分かっているものとする
(実際は、初期値を決めれば、カルマンフィルタで平均値と分散を推定することになる)

$\displaystyle E[x] = \bar{x}, \ E[(x - \bar{x})^2] = \sigma_{x}^2$

$\omega$ についても、下記のように平均と分散が分かっているものとする
こちらは、設計情報から定めたり、事前に無制御状態で計測を行い、求める必要がある

$\displaystyle E[\omega] = \bar{\omega}, \ E[(\omega - \bar{\omega})^2] = \sigma_{\omega}^2$

この条件の中で、下記の一次関数で推定値 $\hat{x}$ を求めることを考え、パラメータ $\alpha, \beta$ を求める

$\displaystyle \hat{x} = \alpha y + \beta$

最小二乗推定法として、下記の推定偏差 $e$ に対する $E[e] = 0$ , $E[(e - \bar{e})^2]$ が最小になるように、パラメータ $\alpha, \beta$ を求める

$\displaystyle \begin{align} e &= x - \hat{x} \\ E[e] &= \bar{e} = 0 \\ E[(e - \bar{e})^2] &= \sigma_{e,min}^2 \end{align}$

まずは、 $E[e] = 0$ の条件から $\beta$ を定める

$\displaystyle \begin{align} E[e] &= E[x - \alpha y - \beta] \\ &= E[x - \alpha (c x + \omega) - \beta] \\ &= \bar{x} - \alpha (c \bar{x} + \bar{\omega}) - \beta \\ \beta &= \bar{x} - \alpha (c \bar{x} + \bar{\omega}) \end{align}$

次に、 $[(e - \bar{e})^2]$ が最小になる条件から、 $\alpha$ を定める

$\displaystyle \begin{align} E[(e - \bar{e})^2] &= E[(x - \alpha y - \beta)^2] \\ &= E[\{x - \alpha (c x + \omega) - \bar{x} + \alpha (c \bar{x} + \bar{\omega})\}^2] \\ &= E[( (x - \bar{x}) - \alpha \{c (x - \bar{x}) + (\omega - \bar{\omega})\} )^2] \\ &= E[\{ (1 - \alpha c)(x - \bar{x}) - \alpha (\omega - \bar{\omega}) \}^2] \\ &= (1 - \alpha c)^2 E[(x - \bar{x})^2] - 2 (1 - \alpha c) \alpha E[(x - \bar{x}) (\omega - \bar{\omega})] + \alpha^2 E[(\omega - \bar{\omega})^2] \\ &= (1 - \alpha c)^2 \sigma_x^2 + \alpha^2 \sigma_\omega^2 \\ \end{align}$

ここで $x$ と $\omega$ が無相関であることから、 $E[(x - \bar{x}) (\omega - \bar{\omega})] = 0$ を利用した
さらに、式を $\alpha$ について整理していく

$\displaystyle \begin{align} E[(e - \bar{e})^2] &= (1 - 2 c \alpha + c^2 \alpha^2) \sigma_x^2 + \alpha^2 \sigma_\omega^2 \\ &= (c^2 \sigma_x^2 + \sigma_\omega^2) \alpha^2 - 2 c \sigma_x^2 \alpha + \sigma_x^2 \\ &= (c^2 \sigma_x^2 + \sigma_\omega^2) (\alpha - \frac{c \sigma_x^2}{c^2 \sigma_x^2 + \sigma_\omega^2})^2 - \frac{c^2 \sigma_x^4}{c^2 \sigma_x^2 + \sigma_\omega^2} + \sigma_x^2 \\ &= (c^2 \sigma_x^2 + \sigma_\omega^2) (\alpha - \frac{c \sigma_x^2}{c^2 \sigma_x^2 + \sigma_\omega^2})^2 + \frac{\sigma_x^2 \sigma_\omega^2}{c^2 \sigma_x^2 + \sigma_\omega^2} \\ \end{align}$

このことから、 $\alpha$ が下記のときに、 $E[(e - \bar{e})^2]$ が最小になる

$\displaystyle \begin{align} \alpha &= \frac{c \sigma_x^2}{c^2 \sigma_x^2 + \sigma_\omega^2} \\ E[(e - \bar{e})^2]_{min} &= \frac{\sigma_x^2 \sigma_\omega^2}{c^2 \sigma_x^2 + \sigma_\omega^2} \\ \end{align}$

結局、 $\hat{x}$ は下記になる

$\displaystyle \begin{align} \hat{x} &= \alpha y + \beta \\ &= \alpha y + \bar{x} - \alpha (c \bar{x} + \bar{\omega}) \\ &= \bar{x} + \frac{c \sigma_x^2}{c^2 \sigma_x^2 + \sigma_\omega^2} \{ y - (c \bar{x} + \bar{\omega}) \} \end{align}$

また、この推定の精度（分散）は、 $E[(e - \bar{e})^2]_{min}$ になる

$\displaystyle \sigma^2 = \frac{\sigma_x^2 \sigma_\omega^2}{c^2 \sigma_x^2 + \sigma_\omega^2}$

また、この後のために、次の表現への変形も行っておく

$\displaystyle \begin{align} \hat{x} &= \bar{x} + \sigma^2 c \sigma_\omega^{-2} \{ y - (c \bar{x} + \bar{\omega}) \} \\ \sigma^2 &= (c^2 \sigma_\omega^{-2} + \sigma_x^{-2})^{-1} \end{align}$

さらに、下記の $A$ , $B$ を定義する

$\displaystyle \begin{align} A &= c^2 \sigma_x^2 + \sigma_\omega^2 \\ B &= c \sigma_x^2 \\ \end{align}$

これを用いると、下記のようにも表現できる

$\displaystyle \begin{align} \alpha &= B A^{-1} \\ \sigma^2 &= \sigma_x^2 - B^2 A^{-1} \\ \end{align}$

多変数の場合

多変数の場合は、 $x$ や $y$ , $\omega$ がベクトル（縦ベクトル）になり、 $C$ の行列になる
基本的にスカラの場合と同じ計算を行うが、行列演算の場合、掛け算や割り算の順番の入れ替えが難しくなる

ここでは、前提の定義と、結果だけを書いておく

$\displaystyle y = C x + \omega \\ E[x] = \bar{x} , \ E[(x - \bar{x}) (x - \bar{x})^T] = \Sigma_{x} \\ E[\omega] = \bar{\omega} , \ E[(\omega - \bar{\omega}) (\omega - \bar{\omega})^T] = \Sigma_{\omega}$

推定モデルは下記

$\displaystyle \hat{x} = F y + d$

条件は、同じく、 $e = x - \hat{x}$ に対して、 $E[e] = 0$ , $E[(e - \bar{e}) (e - \bar{e})^T]$ が最小
$E[(e - \bar{e}) (e - \bar{e})^T]$ の最小値を $\Sigma = P$ とおくと、

$\displaystyle \begin{align} \hat{x} &= \bar{x} + P C^T \Sigma_\omega^{-1} \{ y - (c \bar{x} + \bar{\omega}) \} \\ P &= (C^T \Sigma_\omega^{-1} C + \Sigma_x^{-1})^{-1} \end{align}$

$A$ , $B$ を下記のように定義すると

$\displaystyle \begin{align} A &= C \Sigma_x C^T + \Sigma_\omega \\ B &= C \Sigma_x \\ \end{align}$

下記のようにも表現できる

$\displaystyle \begin{align} F &= B^T A^{-1} \\ P &= \Sigma_x - B^T A^{-1} B \\ \end{align}$

考察

簡単のため、スカラの場合で、 $c = 1$ , $\bar{\omega} = 0$ の場合を考えると
推定値は

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

となり、事前の平均値 $\bar{x}$ と観測値 $y$ の　 $\sigma_x^2$ , $\sigma_\omega^2$ の重みづけ平均になっている

観測ノイズ $\sigma_\omega^2$ が非常に小さい場合は、 $\hat{x} = y$ となり、観測値がそのまま信頼できることになる
逆に、観測ノイズ $\sigma_\omega^2$ が大きい場合は、 $\hat{x} = \bar{x}$ となり、観測値が全然信頼できないことになる