1. はじめに
前回の記事では、カルマンフィルタの基本的な考え方の概要をまとめました。今回は、より詳細にカルマンフィルタを記述する数式に焦点を当て、その背後にある意味合いを一つずつ紐解いていきたいと思います。
2. システムのモデル化
カルマンフィルタを適用するためには、まず対象となるシステムの状態遷移と観測プロセスを数学的にモデル化する必要があります。
2.1. 状態方程式（システムモデル）
システムの状態が時間とともにどのように変化するかを表すのが状態方程式です。離散時間システムの場合、一般的に以下の線形な形で表されます。
x(k) = F(k-1)x(k-1) + B(k-1)u(k-1) + w(k-1)
ここで、
・x(k)：時刻 k における状態ベクトル。推定したい物理量（例：位置、速度、角度など）をまとめたベクトルです。
・F(k-1)：時刻 k-1 から k への状態遷移行列。前の状態がどのように次の状態に影響するかを表します。
・u(k-1)： 時刻 k-1 における制御入力ベクトル。既知の外部からの入力（例：モーターへの電圧、操舵角など）を表します。
・B(k-1): 時刻 k-1 における入力行列。制御入力が状態にどのように影響するかを表します。
・w(k-1)： 時刻 k-1 におけるプロセスノイズベクトル。モデル化されていないシステムの不確実性や外乱を表します。通常、平均 0 の多変量ガウス分布 w(k-1) ~ N(0, Q(k-1)) に従うと仮定されます。Q(k-1)は時刻 k-1 におけるプロセスノイズ共分散行列です。プロセスノイズ共分散行列は、プロセスノイズの強さや相関を表します。
つまり、システムの状態が変化する際に生じる予測できないランダムなノイズ w(k-1) は、複数の状態変数に同時に影響を与える可能性があり、特定の偏りがなく、ばらつき具合や変数間の関連性は共分散行列 Q(k-1) によって特徴づけられるガウス分布に従うと仮定します。
ガウス分布とは?
「ガウス分布」または「正規分布」は、自然界や人間の社会現象において非常によく見られる確率分布の一つです。その特徴的な形から「釣鐘型の分布」とも呼ばれます。多くの自然現象や工学システムにおけるランダムなノイズは、ガウス分布で比較的よく近似できることが知られています。(例外あり）
ガウス分布に従う確率変数は、以下の特徴を持ちます。
・平均値付近の値が出やすい:分布の中心（平均値）に近い値が発生する確率が最も高く、そこから離れるほど確率は低くなります。
・左右対称: 平均値を中心として左右対称な形をしています。
・標準偏差でばらつきが決まる: 分布の広がり具合は「標準偏差」という値で決まります。標準偏差が大きいほど、値が平均値から大きく離れる可能性が高くなります。
多変量とは?
「多変量」とは、複数の要素（変数）が同時に影響し合っていることを意味します。カルマンフィルタでは、状態ベクトル x(k) が通常、複数の状態変数（例えば、位置、速度、角度など）を含んでいます。プロセスノイズ w(k-1) も、これらの複数の状態変数にそれぞれ影響を与える可能性があるため、「多変量」と表現されます。
例えば、飛行型ドローンを例にすると、プロセスノイズは位置のずれだけでなく、速度のわずかな変化や姿勢の乱れなど、複数の状態変数に影響を与える可能性があります。
平均0とは?
「平均 0」とは、ランダムなノイズが、長期的には特定の方向に偏りがないということです。プラス方向にもマイナス方向にも、同じくらいの頻度でランダムに変動すると考えられます。
例えば、飛行型ドローンを例にすると、突風が常に特定の方向に吹くわけではなく、様々な方向にランダムに吹く、というイメージです。
共分散行列 Q(k-1) とは?
「共分散行列 Q(k-1)」は、多変量のガウス分布における「ばらつき」と「変数間の関連性」を表すものです。
・分散: 各状態変数に対応するノイズのばらつきの大きさを表します。対角成分がそれぞれの変数の分散に対応します。
・共分散: 異なる状態変数に対応するノイズが、互いにどのように関連して変動するかを表します。非対角成分が共分散に対応します。
例えば、飛行型ドローンを例にすると、水平方向の位置のノイズと垂直方向の位置のノイズが独立している（互いに影響しない）場合は、共分散は 0 になります。しかし、もしある原因で水平方向と垂直方向のノイズが連動して発生しやすい傾向がある場合は、共分散は 0 以外の値になります。
2.2. 観測方程式（観測モデル）
システムの状態がどのように観測されるかを表すのが観測方程式です。これも一般的に線形な形で表されます。
z(k) = H(k)x(k) + v(k)
ここで、
・z(k): 時刻 k における観測ベクトル。センサーから得られた測定値のベクトルです。
・H(k): 時刻 k における観測行列。状態ベクトルがどのように観測ベクトルに変換されるかを表します。どの状態変数がどのセンサーで観測されるか、またその比例関係などを記述します。
・v(k): 時刻 k における観測ノイズベクトル。センサーの測定誤差やノイズを表します。通常、平均 0 の多変量ガウス分布 v(k) ~ N(0, R(k)) に従うと仮定されます。R(k)は 時刻 k における観測ノイズ共分散行列です。観測ノイズ共分散行列は、観測ノイズの強さやセンサー間の相関を表します。
3. カルマンフィルタのアルゴリズム
カルマンフィルタは、上記のシステムモデルと観測モデルに基づいて、以下の2つの主要なステップを繰り返すことで状態を推定します。
3.1. 予測ステップ
式(1)：状態の事前予測
システムモデルをもとに、現在の状態を予測します。
x̂(k|k-1) = F(k-1)x̂(k-1|k-1) + B(k-1)u(k-1)
x̂(k|k-1)の表記について
「x̂(k|k-1)」は「時刻k-1の状態が発生した上での、時刻kの状態」を表しています。
カルマンフィルタは、理論的にはベイズ推定の枠組みを用いて、過去の情報を事前分布、現在の観測情報を尤度として組み合わせ、事後分布の計算を行うというものです。そのため数式としては、このような条件付き確率の表記となります。
しかし、条件付き確率の概念を背景に持ちつつも、実際に実装する際には、平均と共分散といった統計的な量を用いるので、確率計算をするのではなく行列演算や基本的な統計計算を行います。
式(2)：予測誤差共分散の事前予測
P(k|k-1) = F(k-1)P(k-1|k-1)F(k-1)^T + Q(k-1)
これは、予測の不確実性が時間とともにどのように増大するかを評価するものです。過去の推定誤差にシステムモデルの不確実性（プロセスノイズ Q(k-1)）が加わることで、予測の信頼性が低下することを示しています。F(k-1)^T は F(k-1) の転置行列です。
F(k-1)^T について
F(k-1)^Tを掛ける意味は以下の通りです。
・次元の整合性をとるため:
P(k-1|k-1) は状態ベクトルの次元に関する正方行列です。F(k-1) は状態ベクトル間の変換を行う行列であり、一般には正方行列とは限りません。F(k-1)^T を適切に掛けることで、結果として得られる P(k|k-1) が、現在の状態ベクトルの次元に関する正方行列となり、共分散行列としての性質を満たします。
・ばらつきの適切な伝播:
行列 F(k-1) の各要素が、前の時刻の各状態変数の誤差を現在の各状態変数にどのように影響させるかを考慮する際に、転置行列との積が、分散と共分散の伝播を数学的に正しく記述するために必要となります。
・共分散行列の対称性の維持:
P(k-1|k-1) が対称行列である場合、F(k-1)P(k-1|k-1)F(k-1)^T も対称行列となることが数学的に保証されます。共分散行列は対称性を持つ必要があるので、これは重要な性質です。
3.2. 更新ステップ
式(3)： カルマンゲインの計算
K(k) = P(k|k-1)H(k)^T (H(k)P(k|k-1)H(k)^T + R(k))^-1
カルマンゲイン K(k) は、予測と観測のどちらをどれだけ重視するかを決める「重み」の役割を果たします。
予測誤差共分散 P(k|k-1) が大きい場合（予測の不確実性が高い場合）、カルマンゲインは大きくなり、観測値をより重視します。
観測ノイズ共分散 R(k) が大きい場合（観測の信頼性が低い場合）、カルマンゲインは小さくなり、予測値をより重視します。
(...)^-1 は行列の逆行列を表します。
式(4)： 状態の事後推定
x̂(k|k) = x̂(k|k-1) + K(k)(z(k) - H(k)x̂(k|k-1))
これは、事前予測された状態 x̂(k|k-1) を、項 (z(k) - H(k)x̂(k|k-1)) で修正するものです。
H(k)x̂(k|k-1) は、事前予測された状態に基づいて予測される観測値です。
z(k) - H(k)x̂(k|k-1) は、実際の観測値と予測された観測値の差であり、予測のずれを表しています。
このずれにカルマンゲイン K(k) を掛けることで、予測をどれだけ修正するかが決まります。
式(5)： 推定誤差共分散の事後推定
P(k|k) = (I - K(k)H(k))P(k|k-1)
これは、更新された状態 x̂(k|k) の不確実性を評価するものです。カルマンゲイン K(k) によって観測情報が取り込まれた結果、推定誤差共分散が減少することを示しています。I は単位行列です。
3.3. アルゴリズムの流れ
カルマンフィルタは、初期状態の推定値 x̂(0|0) とその誤差共分散行列 P(0|0) を与えることから始まり、以下の予測ステップと更新ステップを時間 k = 1, 2, ... に対して繰り返します。
・予測ステップ: 式 (1) と (2) を用いて、次の時刻の状態と誤差共分散を予測します。
・更新ステップ: 実際の観測値 z(k) が得られたら、式 (3), (4), (5) を用いて、予測された状態と誤差共分散を更新します。
この繰り返しによって、ノイズを含む観測データから、より正確なシステムの状態推定値が逐次的に得られます。
4. まとめ
今回は、カルマンフィルタの主要な数式とその意味合いについてまとめました。各数式がどのような役割を果たし、予測と観測の情報をどのように統合しているかを理解することで、カルマンフィルタの動作原理をより深く理解できると思います。最後までお読みいただきありがとうございます。