背景

多次元カルマンフィルタに取り組む前に、いくつかの重要な数学のトピックを復習しておく必要があります。

  • 行列計算
  • 共分散と共分散行列
  • 期待値代数

もしこれらのトピックを理解できていれば、次の章に進むことができます。

参考文献

行列計算

行列計算については、次の基本事項を理解できていれば十分です。

  • ベクトル・行列の加法と乗算
  • 転置
  • 逆行列(自分で逆行列の計算ができる必要はありません。逆行列とは何かを理解できていれば十分です。)
  • 対称行列

これらのトピックを扱った線形代数の参考書やWebサイトは多数存在します。

共分散と共分散行列

このトピックについては、visiondummy のサイトがとても参考になります。

https://www.visiondummy.com/2014/04/geometric-interpretation-covariance-matrix/

期待値代数

これから、カルマンフィルタの式の導出において、期待値に関する公式を多く使用していきます。導出の理解を深めるためには、期待値代数をマスターする必要があります。

確率変数とは何か、期待値とは何か、既に理解しているかと思います。そうでなければ、前回の背景のページを読んでください。

基本的な期待値に関する公式

期待値は大文字 \( E \) で表されます。

確率変数の期待値は、その確率変数の平均値と等しくなります。

\[ E(X) = \mu_{X} \]
ここで、\( \mu_{X} \) は確率変数の平均値

以下に、基本的な期待値の公式を記します。

公式 備考
1 \( E(X) = \mu_{X} = \Sigma xp(x) \) \( p(x) \) は \( x \)(離散型確率変数)の確率
2 \( E(a) = a \) \( a \) は一定
3 \( E(aX) = aE(X) \) \( a \) は一定
4 \( E(a \pm X) = a \pm E(X) \) \( a \) は一定
5 \( E(a \pm bX) = a \pm bE(X) \) \( a \) と \( b \) は一定
6 \( E(X \pm Y) = E(X) \pm E(Y) \) \( Y \) は確率変数
7 \( E(XY) = E(X)E(Y) \) \( X \) と \( Y \) が無相関な場合

分散と共分散に関する公式

次の表には、分散と共分散に関する公式を記しています。

公式 備考
8 \( V(a) = 0 \) \( V(a) \) は変数 \( a \) の分散
\( a \) は一定
9 \( V(a \pm X) = V(X) \) \( V(X) \) は変数 \( X \) の分散
\( a \) は一定
10 \( V(X) = E(X^{2}) - \mu_{X}^{2} \) \( V(X) \) は変数 \( X \) の分散
11 \( COV(X,Y) = E(XY) - \mu_{X}\mu_{Y} \) \( COV(X,Y) \) は \( X \) と \( Y \) の共分散
12 \( COV(X,Y) = 0 \) \( X \) と \( Y \) が無相関な場合
13 \( V(aX) = a^{2}V(X) \) \( a \) は一定
14 \( V(X \pm Y) = V(X) + V(Y) \pm 2COV(X,Y) \)
15 \( V(XY) \neq V(X)V(Y) \)

分散と共分散に関する公式は直感的ではありません。そこで、これらのいくつかについて説明を加えたいと思います。

公式 8

\[ V(a) = 0 \]

定数は変化しないので、定数の分散は0です。

公式 9

\[ V(a \pm X) = V(X) \]

変数に定数を加えても、その変数の分散は変化しません。

公式 10

\[ V(X) = E(X^{2}) - \mu_{X}^{2} \]

証明

備考
\( V(X) = \sigma_{X}^2 = E((X - \mu_{X})^2) = \)
\( E(X^2 -2X\mu_{X} + \mu_{X}^2) = \)
\( E(X^2) - E(2X\mu_{X}) + E(\mu_{X}^2) = \) 公式5を適用: \( E(a \pm bX) = a \pm bE(X) \)
\( E(X^2) - 2\mu_{X}E(X) + E(\mu_{X}^2) = \) 公式3を適用: \( E(aX) = aE(X) \)
\( E(X^2) - 2\mu_{X}E(X) + \mu_{X}^2 = \) 公式2を適用: \( E(a) = a \)
\( E(X^2) - 2\mu_{X}\mu_{X} + \mu_{X}^2 = \) 公式1を適用: \( E(X) = \mu_{X} \)
\( E(X^2) - \mu_{X}^2 \)

公式 11

\[ COV(X,Y) = E(XY) - \mu_{X}\mu_{Y} \]

証明

備考
\( COV(X,Y) = E((X - \mu_{X})(Y - \mu_{Y}) \) =
\( E(XY - X \mu_{Y} - Y \mu_{X} + \mu_{X}\mu_{Y}) = \)
\( E(XY) - E(X \mu_{Y}) - E(Y \mu_{X}) + E(\mu_{X}\mu_{Y}) = \) 公式6を適用: \( E(X \pm Y) = E(X) \pm E(Y) \)
\( E(XY) - \mu_{Y} E(X) - \mu_{X} E(Y) + E(\mu_{X}\mu_{Y}) = \) 公式3を適用: \( E(aX) = aE(X) \)
\( E(XY) - \mu_{Y} E(X) - \mu_{X} E(Y) + \mu_{X}\mu_{Y} = \) 公式2を適用: \( E(a) = a \)
\( E(XY) - \mu_{Y} \mu_{X} - \mu_{X} \mu_{Y} + \mu_{X}\mu_{Y} = \) 公式1を適用: \( E(X) = \mu_{X} \)
\( E(XY) - \mu_{X}\mu_{Y} \)

公式 13

\[ V(aX) = a^{2}V(X) \]

証明

備考
\( V(K) = \sigma_{K}^2 = E(K^{2}) - \mu_{K}^2 \)
\( K = aX \)
\( V(K) = V(aX) = E((aX)^{2} ) - (a \mu_{X})^{2} = \) \( K \) に \( aX \) を代入
\( E(a^{2}X^{2}) - a^{2} \mu_{X}^{2} = \)
\( a^{2}E(X^{2}) - a^{2}\mu_{X}^{2} = \) 公式3を適用: \( E(aX) = aE(X) \)
\( a^{2}(E(X^{2}) - \mu_{X}^{2}) = \)
\( a^{2}V(X) \) 公式10を適用: \( V(X) = E(X^{2}) - \mu_{X}^2 \)

等速運動の場合

\[ V(x) = \Delta t^{2}V(v) \] または \[ \sigma_{x}^2 = \Delta t^{2}\sigma_{v}^2 \]
ここで
\( x \) :物体の位置
\( v \) :物体の速度
\( \Delta t \) :時間間隔

公式 14

\[ V(X \pm Y) = V(X) + V(Y) \pm 2COV(X,Y) \]

証明

備考
\( V(X \pm Y) = \)
\( E((X \pm Y)^{2}) - (\mu_{X} \pm \mu_{Y})^{2} = \) 公式10を適用: \( V(X) = E(X^{2}) - \mu_{X}^2 \)
\( E(X^{2} \pm 2XY + Y^{2}) - (\mu_{X}^2 \pm 2\mu_{X}\mu_{Y} + \mu_{y}^2) = \)
\( \color{red}{E(X^{2}) - \mu_{X}^2} + \color{blue}{E(Y^{2}) - \mu_{Y}^2} \pm 2(E(XY) - \mu_{X}\mu_{Y} ) = \) 公式6を適用: \( E(X \pm Y) = E(X) \pm E(Y) \)
\( \color{red}{V(X)} + \color{blue}{V(Y)} \pm 2(E(XY) - \mu_{X}\mu_{Y} ) = \) 公式10を適用: \( V(X) = E(X^{2}) - \mu_{X}^2 \)
\( V(X) + V(Y) \pm 2COV(X,Y) \) 公式11を適用: \( COV(X,Y) = E(XY) - \mu_{X}\mu_{Y} \)

共分散行列と期待値

\( k \) 個の要素を持つベクトル \( \boldsymbol{x} \) を考えます。

\[ \boldsymbol{x} = \left[ \begin{matrix} x_{1}\\ x_{2}\\ \vdots \\ x_{k}\\ \end{matrix} \right] \]

ベクトル \( \boldsymbol{x} \) の共分散行列は次式で与えられます。

\[ COV(\boldsymbol{x}) = E \left( \left( \boldsymbol{x - \mu_{x}} \right) \left( \boldsymbol{x - \mu_{x}} \right)^{T} \right) \]

証明

\[ COV(\boldsymbol{x}) = E \left( \left[ \begin{matrix} (x_{1} - \mu_{x_{1}})^{2} & (x_{1} - \mu_{x_{1}})(x_{2} - \mu_{x_{2}}) & \cdots & (x_{1} - \mu_{x_{1}})(x_{k} - \mu_{x_{k}}) \\ (x_{2} - \mu_{x_{2}})(x_{1} - \mu_{x_{1}}) & (x_{2} - \mu_{x_{2}})^{2} & \cdots & (x_{2} - \mu_{x_{2}})(x_{k} - \mu_{x_{k}}) \\ \vdots & \vdots & \ddots & \vdots \\ (x_{k} - \mu_{x_{k}})(x_{1} - \mu_{x_{1}}) & (x_{k} - \mu_{x_{k}})(x_{2} - \mu_{x_{2}}) & \cdots & (x_{k} - \mu_{x_{k}})^{2} \\ \end{matrix} \right] \right) = \]

\[ = E \left( \left[ \begin{matrix} (x_{1} - \mu_{x_{1}}) \\ (x_{2} - \mu_{x_{2}}) \\ \vdots \\ (x_{k} - \mu_{x_{k}}) \\ \end{matrix} \right] \left[ \begin{matrix} (x_{1} - \mu_{x_{1}}) & (x_{2} - \mu_{x_{2}}) & \cdots & (x_{k} - \mu_{x_{k}}) \end{matrix} \right] \right) = \]

\[ = E \left( \left( \boldsymbol{x - \mu_{x}} \right) \left( \boldsymbol{x - \mu_{x}} \right)^{T} \right) \]