高校数学で紐解くカルマンフィルタ

はじめに

カルマンフィルタは、ロボット工学や信号処理などで使われる「最適推定」の方法の一つです。その本質は、誤差の分散を最小にするアルゴリズム であることにあります。これは、高校数学で習う2次関数の最大・最小問題を解くことと本質的に同じ です。すなわち、誤差の分散を表す2次関数を微分し、その最小値を求めることで、カルマンフィルタの更新式が導かれます。

私たちは、直接観測できない「本当の状態（真値）」を、予測値（事前推定） と 観測値 を上手に組み合わせることで、最も信頼できる推定値を求めます。

本記事では、誤差を最小にするために、予測と観測を加重平均し、最適な重みを求めることで、更新式がどのように導かれるのかを詳しく見ていきます。高校数学で習った2次関数の最大・最小問題を解く感覚で、カルマンフィルタの本質を理解していきましょう。

1. カルマンフィルタの基本モデル（1次モデル）

まず、最もシンプルな1次モデルを考えます。

状態変数 ${ x_k }$ を持つシステムを、次のように表します。

状態の変化（予測式）
$x_k = F x_{k-1} + w_k$
ここで、
- ${ x_k }$ は時刻 ${ k }$ における「真の状態」
- ${ F }$ は状態の変化を表す係数
- ${ w_k }$ はノイズ（予測のズレ）
観測式
$z_k = H x_k + v_k$
ここで、
- ${ z_k }$ は観測値（ノイズが乗っている）
- ${ H }$ は観測のスケールを表す係数
- ${ v_k }$ は観測ノイズ

このように、私たちは「予測値」と「観測値」という 2つの情報 を持っています。この2つを適切に組み合わせて、より良い推定値を求めるのがカルマンフィルタの目的です。

一次モデルで考えると、状態量と観測値はスケールこそ異なるものの、本質的には同じ情報を表していることが直感的に理解できます。たとえば、温度（℃）を測定するセンサが電圧（V）で値を返す場合、20℃がそのまま20Vとして出力されるわけではなく、スケール変換が行われて1/50にされ、0.4Vとして出力されることがあります。このように、観測値は状態量に比例してスケーリングされた形で得られるのです。

ドローンにおいてカルマンフィルタを用いる場合、姿勢（機体の傾きなど）を推定する手法として主に2つの方法があります。

ジャイロセンサから得られる角速度を時間積分して求める方法
加速度センサから得られる重力ベクトルの向きから計算する方法

これらの手法には一長一短があります。ジャイロの積分による姿勢推定は、短時間では精度が高いものの、時間とともに誤差が蓄積しドリフトが生じやすいです。一方、重力ベクトルからの姿勢推定は、長期的には安定していますが、加速度のノイズや飛行中の動きによる外乱の影響を受けやすくなります。

そのため、どちらか一方に依存するのではなく、それぞれの長所を活かし短所を補い合う形で組み合わせる必要があります。このように、複数のセンサ情報を統合して、より正確な推定を行う手法がカルマンフィルタなのです。

2.予測ステップ（事前推定）

前時刻の情報を使って、現在の状態の予測値（事前推定値） ${ \hat{x}_{k|k-1} }$

$\hat{x}_{k|k-1} = F \hat{x}_{k-1}$

誤差分散（共分散行列のスカラー版）も次の様に更新されます

$P_{k|k-1} = F^2 P_{k-1} + Q$

3. 更新ステップ（事後推定）

予測と観測の加重平均

最適な推定値 ${ \hat{x}_k }$ は、「予測値」と「観測値」の加重平均として表せます。

$\hat{x}_k = w_1 \hat{x}_{k|k-1} + w_2 z_k$

ここで、
- ${ \hat{x}_{k|k-1} }$ は 事前推定値（予測による状態推定）
- ${ z_k }$ は 観測値
- ${ w_1 }$ と ${ w_2 }$ は、それぞれの重み

カルマンフィルタでは、推定値の期待値が真の状態 ${ x_k }$ に一致する（不偏推定） ことが求められます。つまり、

${ \mathbb{E} [ \hat{x} _{k} ] = {x}_{k} }$

が成り立つようにしたい。

加重平均の期待値を考える

両辺の期待値を取ると、

$\mathbb{E}[ \hat{x}_k ] = w_1 \mathbb{E}[\hat{x}_{k|k-1}] + w_2 \mathbb{E}[z_k]$

ここで、

事前推定値の期待値は、 ${ \mathbb{E} [ \hat{x}_{k|k-1} ] = x_k }$
観測値の期待値は、 ${ \mathbb{E}[z_k] = H x_k }$ （観測式 ${ z_k = H x_k + v_k }$ より）

これを代入すると、

$x_k = w_1 x_k + w_2 H x_k$

両辺を ${ x_k }$ で割ると、

$1 = w_1 + H w_2$

これが 制約条件として成り立つべき式 であることが分かりり、 この制約を満たすことで、推定値が適切なスケールを持つように補正されます。

誤差の分散を最小にする最適化問題

ここで、誤差（真の状態と推定値の差）の分散を考えます。

推定誤差を

$e_k = x_k - \hat{x}_k$

誤差の分散を計算するために、推定誤差 ${ e_k }$ を展開します。

${ e_k = x_k - (w_1 \hat{x}_{k|k-1} + w_2 z_k) }$

観測値 ${ z_k }$ は、観測モデル

${ z_k = H x_k + v_k }$

を用いて、次のように書き換えられます。

${ e_k = x_k - (w_1 \hat{x}_{k|k-1} + w_2 (H x_k + v_k)) }$

整理すると、

${ e_k = (1 - w_2 H) x_k - w_1 \hat{x}_{k|k-1} - w_2 v_k }$

制約条件 ${w_1 + H w_2=1}$ から ${w_1 =1-w_2 H }$ であるので

${ e_k = w_1 x_k - w_1 \hat{x}_{k|k-1} - w_2 v_k }$

少し整理すると

${ e_k = w_1 (x_k - \hat{x}_{k|k-1}) - w_2 v_k }$

${e_k}$ の分散を ${P_k}$ とすると、 ${P_k}$ は一般的な分散を期待値で表す方法を用いると、以下の様になります。

${ P_k = \mathbb{E}[e_k^2]- \mathbb{E}[e_k] }$

誤差の平均は０になることが期待されているので、 ${P_k}$ は以下となることがわかります。

${ P_k = \mathbb{E}[e_k^2] }$

ここで、 ${e_k}$ の各項の独立性を仮定すると、

$P_k = w_1^2 P_{k|k-1} + w_2^2 R$

ここで

${ P_{k|k-1} }$ は事前推定誤差の分散（推定がどれくらい信頼できるか）
${ R }$ は観測誤差の分散（観測値がどれくらい信頼できるか）

制約条件 ${ w_1 + w_2 H = 1 }$ を用いて、 ${ w_1 }$ を消去すると、

${ P_k = (1 - w_2 H)^2 P_{k|k-1} + w_2^2 R }$

この分散 ${P_k}$ を最小にするように、最適な重み ${ w_2 }$ を求めることで、カルマンゲインが導かれます。

これで ${ w_2 }$ の２次関数の最小値問題となりました。ここからは、高校数学の知識で解くことができます。

${P_k}$ を ${ w_2 }$ で微分し、ゼロとすることで2次関数の最適化問題 として解きます。

$\frac{d}{dw_2} \left( (1 - w_2 H)^2 P_{k|k-1} + w_2^2 R \right) = 0$

展開すると、

$-2(1 - w_2 H) H P_{k|k-1} + 2 w_2 R = 0$

整理すると、

$H P_{k|k-1} = w_2 (H^2 P_{k|k-1} + R)$

両辺を ${ H^2 P_{k|k-1} + R }$ で割ると、

$w_2 = \frac{P_{k|k-1} H}{H^2 P_{k|k-1} + R}$

これが カルマンゲイン ${ K_k }$ になります。

$K_k = \frac{P_{k|k-1} H}{H^2 P_{k|k-1} + R}$

更新則の導出

求めたカルマンゲイン ${ K_k }$ を加重平均式に代入すると、

$\hat{x}_k = \hat{x}_{k|k-1} + K_k (z_k - H \hat{x}_{k|k-1})$

誤差分散の更新式は、

$P_k = (1 - K_k H) P_{k|k-1}$

となります。

4. QとRについて

QとRと線型結合の重みとの関係について

カルマンフィルタを実装する際にQとRを求めておくことは困難な場合があります。少なくないケースでQとRが実装上のパラメータとなることも多いです。

その際に、カルマンフィルタの更新式でQとRがどの様にかかわるのかを明らかにしておくことが大変有効です。

カルマンゲインにカルマンゲインの中身を代入すると推定値の更新式は以下の様になります。

${ \hat{x}_k = w_1 \hat{x}_{k|k-1} + w_2 z_k }$

ここで

${ w_1 = \frac{R}{H^2 (F^2 P_{k-1} + Q) + R}, \quad w_2 = \frac{(F^2 P_{k-1} + Q) H}{H^2 (F^2 P_{k-1} + Q) + R} }$

誤差分散 ${P_k}$ についても、以下の様になります

$P_k = \frac{R (F^2 P_{k-1} + Q)}{H^2 (F^2 P_{k-1} + Q) + R}$

実装におけるQとRの決定について

カルマンフィルタにおける2つの重要なパラメータ、Q（プロセスノイズの分散） と R（観測ノイズの分散） は、推定値がどれだけ「予測値（モデル）」または「観測値（センサー）」を信頼するかを決定する重み付けのバランスに大きく影響します。

(1) Q（予測に対する信頼度）

Qは、システムモデルによる予測がどれだけ不確かであるかを表します。

Qが小さい場合：
- モデルによる予測を信頼している。
- フィルタは観測値よりも予測値を重視する。
- 例：理論的に非常に正確な物理モデルを用いている場合。
Qが大きい場合：
- モデルの予測に誤差が含まれていると見なす。
- 観測値の方を重視して推定値を補正する。
- 例：環境ノイズや外乱が多く、モデルが単純化されている場合。

(2) R（観測に対する信頼度）

Rは、センサーの測定にどれだけノイズが含まれているかを表します。

Rが小さい場合：
- センサーの測定が高精度で信頼できる。
- フィルタは観測値を重視して推定を補正する。
Rが大きい場合：
- センサーの測定はノイズが多く信頼できない。
- フィルタは観測値をあまり信用せず、予測値を優先する。

(3) QとRの比率が重みを決める

カルマンフィルタの更新式で示されるように、予測値と観測値の加重平均によって推定値が決まるため、QとRの相対的な大きさが重要です。

Q ≫ R（予測が不確か、観測が信頼できる）：
- 観測値に強く引っ張られる（観測を重視）
Q ≪ R（予測が信頼できる、観測が不確か）：
- 観測値をあまり反映せず、予測を信頼する（予測を重視）
Q ≈ R（両方同程度）：
- 予測値と観測値の中間を取るようなバランスの取れた推定

(4) 実際の調整におけるポイント

QとRは、センサの性能やモデルの精度に応じて実験的にチューニングされることが多い。
時間的に変化する環境では、QやRを適応的に変化させるアルゴリズム（例：拡張カルマンフィルタや自己適応フィルタ）も利用される。

(5) QRまとめ

パラメータ	値が小さいとき	値が大きいとき
Q（予測誤差）	予測を重視する	観測を重視する
R（観測誤差）	観測を重視する	予測を重視する

QとRはカルマンフィルタの「性格」を決めるチューニングパラメータです。それぞれの意味と役割を理解することで、より効果的な推定や制御を実現できます。

5. 多次元（行列）モデルへの一般化

1次モデルで得られた式を、多次元（行列）系に拡張すると、

予測ステップ

$\hat{x}_{k|k-1} = F \hat{x}_{k-1}$

$P_{k|k-1} = F P_{k-1} F^T + Q$

更新ステップ

$K_k = P_{k|k-1} H^T (H P_{k|k-1} H^T + R)^{-1}$

$\hat{x}_k = \hat{x}_{k|k-1} + K_k (z_k - H \hat{x}_{k|k-1})$

$P_k = (I - K_k H) P_{k|k-1}$

6. まとめ

本記事では、カルマンフィルタの更新則が 高校数学の2次関数の最適化問題 と同じ考え方で導かれることを説明しました。

予測と観測の加重平均の考え方
誤差分散を最小にする最適化問題への変換
最適な重み（カルマンゲイン）の導出
1次モデルから多次元モデルへの一般化

この流れを理解すれば、カルマンフィルタが「誤差を最小にする最適な推定手法」であることが直感的に理解できるはずです！

付録：カルマンフィルタの記号の意味を解説

カルマンフィルタでは、多くの記号が登場します。これらの記号は、初めて学ぶ人にとっては分かりにくいかもしれません。本記事では、カルマンフィルタでよく使われる記号について、その意味をできるだけ直感的に解説します。

1. 状態変数と観測値

カルマンフィルタでは、システムの状態を 状態変数 で表します。

${ x_k }$ ：状態変数
- 時刻 ${ k }$ における「本当の状態」を表す。
- 例えば、ロボットの位置や速度、気温のような物理量。
- しかし、これは通常 直接観測できない。
${ z_k }$ ：観測値
- 時刻 ${ k }$ における「センサーから得られたデータ」。
- 例えば、GPSの測定値や温度計の読み取り値。
- しかし、測定誤差が含まれているため、必ずしも正確ではない。

2. 予測と更新に関する記号

カルマンフィルタでは、「予測」と「更新」の2つのステップを繰り返します。それぞれのステップにおいて、次の記号が使われます。

(1) 事前推定（予測）に関する記号

${ \hat{x}_{k|k-1} }$ ：事前推定値
- 「過去の情報を使って、次の状態を予測したもの」
- 例えば、前の時刻の状態とシステムの動きを考慮して、今の状態を推定する。
${ P_{k|k-1} }$ ：事前誤差分散
- 予測の信頼性を表す。
- 数値が大きいと、予測の信頼性が低いことを意味する。

(2) 更新（補正）に関する記号

${ \hat{x}_k }$ ：事後推定値
- 予測値 ${ \hat{x}_{k|k-1} }$ に観測値 ${ z_k }$ を取り込んで補正した値。
- 観測値の信頼度を考慮し、より正確な状態を推定する。
${ P_k }$ ：事後誤差分散
- 観測を取り入れた後の推定の信頼性。
- 数値が小さいほど、推定の精度が高いことを意味する。
${ K_k }$ ：カルマンゲイン
- 観測値と予測値の「どちらをどれくらい重視するか」を決める係数。
- 予測の誤差が大きい場合は観測値を重視し、観測の誤差が大きい場合は予測値を重視する。

3. システムのダイナミクスに関する記号

カルマンフィルタは、システムの動きを数式で表します。

${ F }$ ：状態遷移行列
- 「前の状態 ${ x_{k-1} }$ から、今の状態 ${ x_k }$ がどのように変化するか」を表す。
- 例えば、移動する物体なら ${ F = 1 }$ の場合は速度一定、 ${ F > 1 }$ なら加速していることを意味する。
${ H }$ ：観測行列
- 状態変数 ${ x_k }$ がどのように観測されるかを表す。
- 例えば、温度センサーが「本当の気温の2倍の値を測る」なら、 ${ H = 2 }$ となる。

4. ノイズに関する記号

現実のシステムにはノイズ（誤差）が含まれます。カルマンフィルタでは、2種類のノイズを考えます。

${ w_k }$ ：プロセスノイズ（システムが不確定に変化する誤差）
- 例えば、車の移動において、風や摩擦による影響で微妙に位置がずれる。
- この影響を考慮するために、分散 ${ Q }$ を導入する。
${ v_k }$ ：観測ノイズ（センサーの誤差）
- 例えば、GPSの測定誤差。
- この影響を考慮するために、分散 ${ R }$ を導入する。
${ Q }$ ：プロセスノイズの分散
- 予測の誤差の大きさを表す。
- システムの変動が激しい場合は ${ Q }$ が大きくなる。
${ R }$ ：観測ノイズの分散
- センサーの測定誤差の大きさを表す。
- センサーが精度が悪い場合は ${ R }$ が大きくなる。

5. まとめ

カルマンフィルタの記号を簡単にまとめると、

記号	意味
${ x_k }$	真の状態
${ z_k }$	観測値（センサーの測定値）
${ \hat{x}_{k\|k-1} }$	事前推定値（予測）
${ \hat{x}_k }$	事後推定値（更新後）
${ P_{k\|k-1} }$	事前誤差分散（予測の不確かさ）
${ P_k }$	事後誤差分散（更新後の不確かさ）
${ K_k }$	カルマンゲイン（重み）
${ F }$	状態遷移行列（システムの動き）
${ H }$	観測行列（観測のスケール）
${ Q }$	プロセスノイズの分散（システムの誤差）
${ R }$	観測ノイズの分散（測定誤差）