背景

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

• 行列計算
• 期待値代数
• 多変量正規分布

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

例に基づくカルマンフィルタ解説ガイド

本を入手する

行列計算

次の基本用語と演算について理解していれば十分です。

• ベクトルと行列の加算と乗算
• 行列の転置
• 逆行列（自分で逆行列を計算する必要はありません。逆行列とは何かを知っていれば十分です）
• 対称行列
• 固有値と固有ベクトル

これらのトピックを扱う線形代数の教科書やWebチュートリアルは多数あります。

期待値代数

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

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

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

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

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

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

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

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

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

公式 8

\[ V(a) = 0 \]

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

公式 9

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

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

公式 10

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

証明

公式 11

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

証明

公式 13

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

証明

等速運動の場合

公式 14

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

証明

多変量正規分布

はじめに

カルマンフィルタの出力は確率変数であることを確認しました。確率変数の平均は状態推定値を表します。確率変数の分散は推定の不確かさ（不確実性）です。カルマンフィルタは、推定値とその推定値に対する信頼度を提供します。

1次元カルマンフィルタの式には、4つの不確かさの変数が含まれています。

• \( p_{n,n} \) は推定値（現在の状態）の分散
• \( p_{n+1,n} \) は予測値（次の状態）の分散
• \( r_{n} \) は観測分散
• \( q \) はプロセスノイズ

多変量カルマンフィルタの場合、システムの状態は複数の変数を持つベクトルで記述されます。例えば、平面上の物体の位置は、x位置とy位置の2つの変数で記述できます。

例に基づくカルマンフィルタ解説ガイド

本を入手する

\[ \boldsymbol{x} = \left[ \begin{matrix} x\\ y\\ \end{matrix} \right] \]

カルマンフィルタの出力は多変量確率変数です。共分散行列は、多変量確率変数の不確かさの二乗記述します。

多変量カルマンフィルタの不確かさの変数は以下の通りです。

• \( \boldsymbol{P}_{n,n} \) は推定値の不確かさの二乗を記述する共分散行列
• \( \boldsymbol{P}_{n+1,n} \) は予測値の不確かさの二乗を記述する共分散行列
• \( \boldsymbol{R}_{n} \) は観測の不確かさの二乗を記述する共分散行列
• \( \boldsymbol{Q} \) はプロセスノイズを記述する共分散行列

この章では、多変量正規分布と共分散行列について解説します。

共分散

共分散は、2つ以上の確率変数の組の間の相関の強さを示す尺度です。

\( x-y \) 平面上の物体の位置の測定データセットを想定します。

ランダム誤差により、測定値にはばらつき（分散）があります。

いくつかの異なる測定データセットの例を見てみましょう。

上の2つのサブプロットは、無相関な測定値を示しています。\( x \) の値は \( y \) の値に依存しません。青色のデータセットでは、\( x \) と \( y \) の値は同じ分散を持ち、円形の分布形状が見られます。赤色のデータセットでは、\( x \) の分散は \( y \) の分散よりも大きく、分布形状は楕円形です。

測定値が無相関であるため、\( x \) と \( y \) の共分散はゼロになります。

下の2つのサブプロットは、相関のある測定値を示しています。\( x \) と \( y \) の値の間には依存関係があります。緑色のデータセットでは、\( x \) が増加すると \( y \) も増加し、その逆も同様です。相関は正であるため、共分散は正になります。シアン色のデータセットでは、\( x \) が増加すると \( y \) は減少し、その逆も同様です。相関は負であるため、共分散は負になります。

サイズ \( N \) の母集団 \( X \) と母集団 \( Y \) の間の共分散は次式で与えられます。

\[ COV(X,Y) = \frac{1}{N}\sum_{i=1}^{N}(x_{i} - \mu_{x})(y_{i} - \mu_{y}) \]

共分散の式を書き換えてみましょう。

サイズ \( N \) の標本の共分散は \( N-1 \) で正規化されます。

\[ COV(X,Y) = \frac{1}{N-1}\sum_{i=1}^{N}(x_{i} - \mu_{x})(y_{i} - \mu_{y}) \]

共分散の式を書き換えてみましょう。

例：

次のサンプルが与えられたとします。

\[ \boldsymbol{x} = \left[ \begin{matrix} 2 \\ 3 \\ -1 \\ 4 \end{matrix} \right] \]

\[ \boldsymbol{y} = \left[ \begin{matrix} 8 \\ 7 \\ 9 \\ 6 \end{matrix} \right] \]

\[ COV(X,Y) = \frac{1}{N-1}\sum_{i=1}^{N}(x_{i}y_{i}) - \frac{N}{N-1}\mu_{x}\mu_{y} = \]

\[ = \frac{1}{3} \left( 2 \times 8 + 3 \times 7 - 1 \times 9 + 4 \times 6 \right) - \frac{4}{3} \left( \frac{(2+3-1+4)}{4} \frac{(8+7+9+6)}{4} \right) = -2.67 \]

サンプルを2つのベクトル \( \boldsymbol{x} \) と \( \boldsymbol{y} \) に並べることができます。ベクトル表記による共分散は次式で与えられます。

\[ COV(X,Y) = \frac{1}{N-1}\boldsymbol{x}^{T}\boldsymbol{y} - \frac{N}{N-1}\mu_{x}\mu_{y} \]

平均ゼロの確率変数の場合、共分散は次式で与えられます。

\[ COV(X,Y) = \frac{1}{N-1}\boldsymbol{x}^{T}\boldsymbol{y} \]

共分散行列

共分散行列は、与えられた多変量確率変数の要素の各ペア間の共分散を表す正方行列です。

2次元の確率変数の場合、共分散行列は以下のようになります。

\[ \boldsymbol{\Sigma} = \left[ \begin{matrix} \sigma_{xx} & \sigma_{xy} \\ \sigma_{yx} & \sigma_{yy} \\ \end{matrix} \right] = \left[ \begin{matrix} \sigma_{x}^{2} & \sigma_{xy} \\ \sigma_{yx} & \sigma_{y}^{2} \\ \end{matrix} \right] = \left[ \begin{matrix} VAR(\boldsymbol{x}) & COV(\boldsymbol{x, y}) \\ COV(\boldsymbol{y, x}) & VAR(\boldsymbol{y}) \\ \end{matrix} \right] \]

\( COV(\boldsymbol{x, y}) = COV(\boldsymbol{y, x}) \) であるため、共分散行列の非対角成分は等しいことに注意してください。もし \( x \) と \( y \) が無相関であれば、共分散行列の非対角成分はゼロになります。

\( n \) 次元確率変数の場合、共分散行列は以下のようになります。

\[ \boldsymbol{\Sigma} = \left[ \begin{matrix} \sigma_{1}^{2} & \sigma_{12} & \cdots & \sigma_{1n} \\ \sigma_{21} & \sigma_{2}^{2} & \cdots & \sigma_{2n} \\ \vdots & \vdots & \ddots & \vdots \\ \sigma_{n1} & \sigma_{n2} & \cdots & \sigma_{n}^{2}\\ \end{matrix} \right] \]

多くの科学計算パッケージで共分散行列を計算できます。

import numpy as np

x = np.array([2, 3, -1, 4])
y = np.array([8, 7, 9, 6])

C = np.cov(x,y)
print(C)

[[ 4.66666667 -2.66666667]
 [-2.66666667  1.66666667]]
                    

x = [2 3 -1 4];
y = [8 7 9 6];

C = cov(x,y)

C =

    4.6667   -2.6667
   -2.6667    1.6667
                    

共分散行列の性質

1. 共分散行列の対角成分は、多変量確率変数の各成分の分散です。

\[ \boldsymbol{\Sigma}_{ii} = \sigma^{2}_{i} \]

2. 対角成分はすべて非負であるため、この共分散行列のトレース（対角成分の和）は非負になります。

\[ tr(\boldsymbol{\Sigma}) = \sum^{n}_{i=1}\boldsymbol{\Sigma}_{ii} \geq 0 \]

3. \( \boldsymbol{\Sigma}_{ij} = \sigma_{ij} = \sigma_{ji} = \boldsymbol{\Sigma}_{ji} \) であるため、共分散行列は対称です。

\[ \boldsymbol{\Sigma} = \boldsymbol{\Sigma}^{T} \]

4. 共分散行列は半正定値です。
   任意のベクトル \( \boldsymbol{v} \neq 0 \) に対して \( \boldsymbol{v}^{T}\boldsymbol{A}\boldsymbol{v} \geq 0 \) であるとき、行列 \( \boldsymbol{A} \) は半正定値と呼ばれます。
   \( \boldsymbol{A} \) の固有値は非負です。

共分散行列と期待値

\( 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[ \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} - \boldsymbol{\mu}_{x} \right) \left( \boldsymbol{x} - \boldsymbol{\mu}_{x} \right)^{T} \right) \]

多変量正規分布

すでに1変量の正規分布についてはよくご存知でしょう。これは釣鐘型のガウス関数で記述されます。

\[ p(x|\mu,\sigma) = \frac{1}{\sqrt{2\pi\sigma^{2}}} exp \left( -\frac{(x-\mu)^{2}}{2\sigma^{2}} \right)\]

正規分布は次のように表記されます。

\[ \mathcal{N}(\mu,\sigma^{2})\]

多変量正規分布は、1変量正規分布を多次元確率変数へ一般化したものです。

\( n \) 次元多変量正規分布は、次のように記述されます。

\[ p(\boldsymbol{x}|\boldsymbol{\mu},\boldsymbol{\Sigma}) = \frac{1}{\sqrt{(2\pi)^{n}|\boldsymbol{\Sigma}|}} exp \left( -\frac{1}{2}(\boldsymbol{x} - \boldsymbol{\mu})^{T}\boldsymbol{\Sigma}^{-1}(\boldsymbol{x} - \boldsymbol{\mu}) \right)\]

ここで:

• \( \boldsymbol{x} \) は \( n \) 次元確率ベクトル
• \( \boldsymbol{\mu} \) は \( n \) 次元平均ベクトル
• \( \boldsymbol{\Sigma} \) は \( n \times n \) の正方共分散行列

2変量正規分布

2変量（2次元）正規分布は、2つの正規分布する確率変数から成ります。ここでは、多変量正規分布の中でも私たちが可視化できる最も高い次元である2変量正規分布に焦点を当てたいと思います。

以下のプロットは、2変量ガウス関数を表しています。

信頼区間

信頼区間とは、1変量正規分布において、パラメータが平均値周辺のある値の範囲内に収まる確率を規定するものです。

1変量正規分布の場合、\( 1\sigma \) 境界とガウス関数の間の面積は、ガウス関数の下の総面積の 68.26% です。

1変量正規分布については、次のように言えます。

• 確率 68.26% の信頼区間は \( 1\sigma \) である。
• 確率 95.44% の信頼区間は \( 2\sigma \) である。
• 確率 99.74% の信頼区間は \( 3\sigma \) である。

1変量正規分布の任意の確率に対する信頼区間を求めることもできます。説明はこちら（英語）をご覧ください。

2変量正規分布の確率は、3次元ガウス関数の体積となります。

例えば、\( 1\sigma \) レベルでスライスした3次元ガウス関数の体積は、3次元ガウス関数の総体積の 39.35% です。

3次元ガウス関数のスライスの投影は楕円になります。

共分散楕円

まず、共分散楕円の性質について見てみましょう。共分散楕円はガウス分布の等高線を表し、2次元における \( 1\sigma \) 信頼区間を視覚化できます。共分散楕円は、共分散行列の幾何学的な解釈を提供します。

任意の楕円は4つのパラメータで記述できます。

• 楕円の中心 \( \mu_{x}, \mu_{y} \)
• 長半径 \( a \)
• 短半径 \( b \)
• 回転角 \( \theta \)

楕円の中心は確率変数の平均です。

\[ \mu_{x} = \frac{1}{N}\sum^{N}_{i=1}x_{i} \]

\[ \mu_{y} = \frac{1}{N}\sum^{N}_{i=1}y_{i} \]

楕円の軸の長さは、確率変数の共分散行列の固有値の平方根です。

• 長半径 \( a \) の長さは、最大の固有値の平方根で与えられます。
• 短半径 \( b \) の長さは、2番目の固有値の平方根で与えられます。

楕円の向きは、最大の固有値に対応する共分散行列の固有ベクトルの向きです。

\[ \theta = arctan \left( \frac{v_{y}}{v_{x}} \right)\]

ここで:

• \( v_{x} \) は最大固有値に対応する固有ベクトルの \(x\) 座標
• \( v_{y} \) は最大固有値に対応する固有ベクトルの \(y\) 座標

科学計算パッケージを使用して、共分散楕円のパラメータを計算できます。

import numpy as np

C = np.array([[5, -2],[-2, 1]]) # define covariance matrix

eigVal, eigVec = np.linalg.eig(C) # find eigenvalues and eigenvectors

a = np.sqrt(eigVal[0]) # half-major axis length
b = np.sqrt(eigVal[1]) # half-minor axis length

# ellipse  orientation  angle
theta = np.arctan(eigVec[1, 0] / eigVec[0, 0])  
												

C = [5 -2; -2 1]; % define covariance matrix

[eigVec, eigVal] = eig(C); % find eigenvalues and eigenvectors


if eigVal(1,1) > eigVal(2,2) % get the highest eigenvalue index

    a = sqrt(eigVal(1,1)); % half-major axis length
    b = sqrt(eigVal(2,2)); % half-minor axis length

    theta = atan(eigVec(2,1) / eigVec(1,1));  % ellipse angle (radians)
else

    a = sqrt(eigVal(2,2)); % half-major axis length
    b = sqrt(eigVal(1,1)); % half-minor axis length

    theta = atan(eigVec(2,2) / eigVec(2,1));  % ellipse angle (radians)
end
												

信頼楕円

多くのアプリケーションにおいて、特定の確率の境界を見つけることに関心があります。例えば、95% の確率であれば、ガウス関数の体積の 95% を含む境界を見つける必要があります。

この境界を \( x-y \) 平面に投影したものが信頼楕円です。

私たちは、共分散楕円を 95% の確率に関連付けられた信頼楕円に拡張する、楕円スケール係数 \( k \) を見つけたいと考えています。

\( \sigma_{x} \) と \( \sigma_{y} \) は確率的に独立な確率変数の標準偏差を表すため、カイ二乗分布の加法定理を使用して、信頼楕円に関連する確率が以下で与えられることを示すことができます。

\[ p = 1 - exp \left( -\frac{1}{2}k^{2} \right) \]

共分散楕円の場合 \( k = 1 \) であるため、共分散楕円に関連する確率は次のようになります。

\[ p = 1 - exp \left( -\frac{1}{2} \right) = 39.35\% \]

与えられた確率に対して、楕円スケール係数を求めることができます。

\[ k = \sqrt{-2ln(1-p)} \]

確率 95% の場合:

\[ k = \sqrt{-2ln(1-0.95)} = 2.45\]

95% の確率に関連付けられた信頼楕円の性質は以下の通りです。

• 楕円の中心 \( (\mu_{x}, \mu_{y}) \) は共分散楕円と同じ
• 回転角 \( \theta \) は共分散楕円と同じ
• 長半径の長さは \( 2.45a \) – 共分散楕円の長半径をスケーリングしたもの
• 短半径の長さは \( 2.45b \) – 共分散楕円の短半径をスケーリングしたもの