1次元カルマンフィルタ

この章では、1次元の変数に対するカルマンフィルタについて説明します。この章の主な目的は、複雑で混乱しそうな数学を使わずに、カルマンフィルタの概念を「簡単で直感的な方法」で説明することです。

これから、カルマンフィルタの式に向かって一歩一歩進んでいきましょう。

はじめに、プロセスノイズのない簡単な例でカルマンフィルタの方程式を導出します。
次に、プロセスノイズを加えたモデルを考えます。

プロセスノイズのない1次元カルマンフィルタ

前章でも述べたように、カルマンフィルタは5つの方程式に基づいています。私たちはそのうちの2つをすでに扱いました。

状態更新式
状態方程式（システムモデル）

この章では、残りの3つの方程式を導出します。

最初の例題（金の延べ棒の重さの測定）を思い出しましょう。私たちは複数の観測を行い、それらを平均して推定値を計算しました（カルマンフィルタの分野では、測定のことを観測と呼ぶことが多いので、今後は観測と呼ぶことにします）。

その結果を次のグラフに示します。

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

本を入手する

Measurements vs. True value vs. Estimates

上のグラフでは、真値、推定値、観測値を、各観測回数に対して見ることができます。

観測値（青）と真値（緑）の差が観測誤差です。観測誤差はランダムなので、分散 ( \( \sigma ^{2} \) ) で記述することができます。観測誤差の分散は、使用した秤のマニュアルを見るか、校正手順によって導き出すことができます。観測誤差の分散は、実際には観測の不確かさです。

注意：文献によっては、観測の不確かさを観測誤差と呼ぶこともあります

以下、観測の不確かさを \( r \) と表すことにします。

推定値（赤）と真値（緑）の差が推定誤差です。見ての通り、観測回数を重ねるごとに推定誤差は小さくなり、0に向かって収束していきますが、推定値は真値に向かって収束していきます。推定誤差は分かりませんが、推定の不確かさは推定できます。

以下、推定の不確かさを \( p \) と表すことにします。

では、体重測定のPDF (Probability Density Function、確率密度関数) を見てみましょう。

次のグラフは、金の延べ棒の重さを10回観測したものです。

観測値を青の線で示しています。
真値を赤の点線で示しています。
観測値の確率密度関数は緑の線で示しています。
緑色で塗られら領域は、観測値の標準偏差 ( \( \sigma \) ) を示しており、真値がこの範囲にある確率は68.26%です。

見てわかるように、10回中8回は真値に近い値を観測しているので、真値は \( 1 \sigma \) の領域の中にあります。

観測の不確かさ ( \( r \) ) は、観測の分散 ( \( \sigma ^{2} \) ) です。

Measurements Probability Density Function

1次元におけるカルマンゲイン

次に、3つ目の式である、カルマンゲインの式を導きます。ここでは、カルマンゲインの式の直感的な導出を紹介します。数学的な導出は次章以降にします。

カルマンフィルタでは反復計算において、\( \alpha \) -\( \beta \) (-\( \gamma \) ) パラメータが動的に計算されます。これらのパラメータはカルマンゲインと呼ばれ、\( K_{n} \) と表記されます。

カルマンゲインの式は以下の通りです。

\[ K_{n}= \frac{Uncertainty \quad in \quad Estimate}{Uncertainty \quad in \quad Estimate \quad + \quad Uncertainty \quad in \quad Measurement}= \frac{p_{n,n-1}}{p_{n,n-1}+r_{n}} \]

ここで、

\( p_{n,n-1} \)	推定の不確かさ
\( r_{n} \)	観測の不確かさ

カルマンゲインは0から1の間の値を取ります。

\[ 0 \leq K_{n} \leq 1 \]

では、カルマンゲインを用いて、状態更新式を書き換えてみましょう。

\[ \hat{x}_{n,n}=~ \hat{x}_{n,n-1}+ K_{n} \left( z_{n}- \hat{x}_{n,n-1} \right) = \left( 1-K_{n} \right) \hat{x}_{n,n-1}+ K_{n}z_{n} \]

ご覧のように、カルマンゲイン \( \left( K_{n} \right) \) は観測値に対する重みであることがわかります。そして、\( \left( 1-K_{n} \right) \) は推定値に対する重みになります。

観測の不確かさが非常に大きく、推定の不確かさが非常に小さい場合、カルマンゲインは0に近くなります。したがって、推定値に大きな重みを与え、観測値には小さな重みを与えます。

一方、観測の不確かさが非常に小さく、推定の不確かさが非常に大きい場合、カルマンゲインは1に近くなります。したがって、推定値に小さな重みを与え、観測値には大きな重みを与えます。

観測の不確かさが推定の不確かさと等しい場合、カルマンゲインは0.5に等しくなります。

カルマンゲインは、観測値が与えられたときに、どれだけ推定値を変化させるかを示します。

カルマンゲインの式は、3つ目のカルマンフィルタの式です。

1次元における推定の不確かさの更新

推定の不確かさの更新は以下の式で定義されます。

\[ p_{n,n}=~ \left( 1-K_{n} \right) p_{n,n-1} \]

ここで、

\( K_{n} \)	カルマンゲイン
\( p_{n,n-1} \)	前回のフィルタ計算で得られた推定の不確かさ
\( p_{n,n} \)	現時点における状態推定の不確かさ

この式は、現時点における状態推定の不確かさを更新します。これは共分散更新式と呼ばれています。なぜ共分散なのでしょうか？その理由は次の章で解説します。

また、\( p_{n,n-1} \)は事前共分散、\( p_{n,n} \)は事後共分散と呼ばれます。

この式から明らかなように、\( \left( 1-K_{n} \right) \leq 1 \) であるために、推定値の不確かさはフィルタの反復ごとに常に小さくなります。観測値の不確かさが大きい場合、カルマンゲインは小さくなり、したがって推定の不確かさの収束は遅くなります。しかし、観測の不確かさが小さい場合は、カルマンゲインが大きくなり、推定値の不確かさは速やかに0に収束します。

共分散更新式は、4つ目のカルマンフィルタの式です。

1次元における推定の不確かさの時間発展

状態の時間発展と同様に、推定の不確かさの時間発展は状態方程式（システムモデル）を用いて行われます。

2つ目の例題である1次元レーダ追尾問題の場合、予測されるターゲット位置は次のようになります。

\[ \hat{x}_{n+1,n}= \hat{x}_{n,n}+ \Delta t\hat{\dot{x}}_{n,n} \] \[ \hat{\dot{x}}_{n+1,n}= \hat{\dot{x}}_{n,n} \]

すなわち、予測位置は現在の推定位置と現在の推定速度に時間を乗じたものに等しくなります。予測される速度は、現在の推定速度に等しくなります（等速モデルを仮定します）。

推定の不確かさの時間発展は次のようになります。

\[ p_{n+1,n}^{x}= p_{n,n}^{x} + \Delta t^{2} \cdot p_{n,n}^{v} \] \[ p_{n+1,n}^{v}= p_{n,n}^{v} \]

Where:

\( p^{x} \)	位置推定の不確かさ
\( p^{v} \)	速度推定の不確かさ

すなわち、次時点の位置推定の不確かさは、現時点の位置推定の不確かさに、現時点の速度推定の不確かさに時間の2乗を掛けたものを加えた値に等しくなります。次時点の速度推定の不確かさは、現時点の速度推定の不確かさに等しくなります（等速モデルを仮定します）。

注意：\( p_{n+1,n}^{x}= p_{n,n}^{x} + \Delta t^{2} \cdot p_{n,n}^{v} \)において、なぜ時間が2乗されているのだろうと疑問に思った方は、分散の期待値の導出を参照してください。

最初の例（金の延べ棒の重量測定）では、システムのダイナミクスは一定です。したがって、推定の不確かさの時間発展は次のようになります。

\[ p_{n+1,n}= p_{n,n} \]

ここで、

\( p \)

金の延べ棒の重量推定の不確かさ

推定値の不確かさの時間発展式は、5つ目のカルマンフィルタの式です。

カルマンフィルタの式のまとめ

この章では、これらの式を1つのアルゴリズムにまとめます。カルマンフィルターは、\( \alpha \)、\( \beta \)、(\( \gamma \) ) フィルタと同様に、「観測、更新、予測」アルゴリズムを利用しています。

次の図は、アルゴリズムの概略を説明したものです。

Schematic description of the Kalman Filter algorithm

フィルタの入力は次の通りです。

初期化

初期化は1回だけ実行され、次の2つのパラメータが与えられます。

初期状態量 ( \( \hat{x}_{1,0} \) )
初期状態量の不確かさ ( \( p_{1,0} \) )

初期化パラメータは、システムやプロセス（例えば、レーダーにおける探索プロセス）、または経験や理論的知識に基づいて推定することが可能です。初期化パラメータが正確ではなくても、カルマンフィルタは実測値に近い値に収束させることができます。

観測

観測は毎ステップにおいて行われます。観測では次の2つのパラメータが与えられます。

観測値 ( \( z_{n} \) )
観測値の不確かさ ( \( r_{n} \) )

カルマンフィルタは、観測値に加えて、観測値の不確かさのパラメータを必要とします。通常、このパラメータは観測に使用した機器のデータシートから提供されるか、観測機器の校正によって得られます。レーダによる観測の不確かさは、SNR（信号対雑音比）、ビーム幅、帯域幅、目標到達時間、クロックの安定性など、様々なパラメータに依存しています。レーダによる観測は、SNR、ビーム幅、ターゲットに到達するまでの時間がそれぞれ異なります。そのため、レーダは各観測に対して観測の不確かさを計算します。

フィルタの出力は次の通りです。

状態推定値 ( \( \hat{x}_{n,n} \) )
推定値の不確かさ ( \( p_{n,n} \) )

カルマンフィルタは、状態推定値に加えて、推定値の不確かさも与えてくれます! すでに述べたように、推定値の不確かさは次式で与えられます。

\[ p_{n,n}=~ \left( 1-K_{n} \right) p_{n,n-1} \]

また、\( \left( 1-K_{n} \right) \leq 1 \) であるために、\( p_{n,n} \) は毎ステップで減少します。

ですから、何回観測をするかは私たち次第です。建物の高さを測定する場合、3cmの精度 ( \( \sigma \) )で測定を行いたいのであれば、推定値の不確かさ ( \( \sigma ^{2} \) ) が \( 9 centimeters^{2} \)以下になるまで測定することになります。

次の表に、5つのカルマンフィルタの式をまとめました。

方程式	名前	文献等で使用される名前
\( \hat{x}_{n,n}=~ \hat{x}_{n,n-1}+ K_{n} \left( z_{n}- \hat{x}_{n,n-1} \right) \)	状態更新式	フィルタリング方程式 ( Filtering Equation )
\( \hat{x}_{n+1,n}= \hat{x}_{n,n}+ \Delta t\hat{\dot{x}}_{n,n} \) \( \hat{\dot{x}}_{n+1,n}= \hat{\dot{x}}_{n,n} \) ( 等速モデル )	状態方程式	遷移方程式 ( Transition Equation ) 予測方程式 ( Prediction Equation ) 力学モデル ( Dynamic Model ) 状態空間モデル ( State Space Model )
\( K_{n}= \frac{p_{n,n-1}}{p_{n,n-1}+r_{n}} \)	カルマンゲイン	重み方程式 ( Weight Equation )
\( p_{n,n}=~ \left( 1-K_{n} \right) p_{n,n-1} \)	共分散更新式	修正方程式 ( Corrector Equation )
\( p_{n+1,n}= p_{n,n} \) (持続予測モデル)	共分散遷移式	予測共分散方程式 ( Predictor Covariance Equation )

注1：状態方程式や共分散遷移式はシステムのダイナミクスに依存しています。

注2：上の表は、カルマンフィルタの方程式を特殊なケースに合わせたものです。この方程式の一般的な形は、後に行列表記で表されます。今はカルマンフィルタの概念を理解することが目標です。

カルマンフィルタのブロック線図を次の図で示します。

Detailed description of the Kalman Filter algorithm

Step 0: 初期化

上記で述べたように、初期化は1回のみ実行され、2つのパラメータが与えられます。

初期状態量 ( \( \hat{x}_{1,0} \) )
初期状態量の不確かさ ( \( p_{1,0} \) )

初期化の後に、予測を行います。

Step 1: 観測

観測プロセスでは次の2つのパラメータが与えられます。

観測値 ( \( z_{n} \) )
観測値の不確かさ ( \( r_{n} \) )

Step 2: 状態更新

状態更新プロセスでは、システムの現在の状態を推定します。

状態更新プロセスの入力は次の通りです。

観測値 ( \( z_{n} \) )
観測値の不確かさ ( \( r_{n} \) )
事前推定値 ( \( \hat{x}_{n,n-1} \) )
事前共分散 ( \( p_{n,n-1} \) )

これらの入力に基づいて、状態更新プロセスはカルマンゲインと次の2つの出力を計算します。

事後推定値 ( \( \hat{x}_{n,n} \) )
事後共分散 ( \( p_{n,n} \) )

これらの2つのパラメータがカルマンフィルタの出力になります。

Step 3: 予測

予測プロセスは、システムのシステムモデルに基づいて、現時点のシステムの状態量と推定値の不確かさを、次時点の状態へ遷移させるものです。

最初のフィルタリング処理において、初期化された出力は事前推定値・事前共分散として使用されます。

その後のフィルタリング処理では、予測プロセスにおいて得られた予測値が事前推定値・事前共分散として使用されます。

カルマンゲインの直感的な説明

カルマンゲインは新しい推定値を計算する際に、観測値の重みと事前推定値の重みを定義する。

カルマンゲインが大きい場合

推定値の不確かさよりも観測の不確かさが小さい場合、カルマンゲインは大きく（1に近く）なります。これは、新しい推定値が観測値に近くなることを意味します（つまり、観測値を信用します）。次の図は、航空機の追跡アプリケーションにおいて、大きいカルマンゲインが推定値に与える影響を示しています。

カルマンゲインが小さい場合

推定値の不確かさよりも観測の不確かさが大きい場合、カルマンゲインは小さく（0に近く）なります。これは、新しい推定値が事前推定値に近くなることを意味します（つまり、事前推定値を信用します）。次の図は、航空機の追跡アプリケーションにおいて、小さいカルマンゲインが推定値に与える影響を示しています。

今、私たちはカルマンフィルタのアルゴリズムを理解し、最初の数値例に取り組む準備ができました。

注：カルマンゲインの数学的背景に興味がある場合は、1次元カルマンゲインの導出を参照してください。

Example 5 – ビルの高さの推定

あまり正確でない高度計を用いて、ビルの高さの推定を行います。

少なくとも短時間の観測では、ビルの高さは時間と共に変化しません。

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

本を入手する

数値例

真のビルの高さは 50 m です。
高度計の観測誤差（標準偏差）は 5 m です。
10回の観測値は、49.03 m, 48.44 m, 55.21 m, 49.98 m, 50.6 m, 52.61 m, 45.87 m, 42.64 m, 48.26 m, 55.84 m です。

反復開始前

初期化

この例題では、初期推定値はビルを眺めることで決定できます。

ビルの高さは、

\[ \hat{x}_{0,0}=60m \]

と推定できます。また、推定値の不確かさの初期値を決定します。人間の推定誤差（標準偏差）は約15 m ( \( \sigma = 15 \) ) と考えられます。従って、分散は \( \sigma ^{2}=225 \) です。

\[ p_{0,0} = 225m^{2} \]

予測

初期値に基づいて、次の状態を推定します。

このシステムは持続予測モデルです。すなわち、ビルの高さは常に一定です。

\[ \hat{x}_{1,0}=\hat{x}_{0,0}= 60m \]

また、推定される分散も変化しません。

\[ p_{1,0}= p_{0,0}=225m^{2} \]

反復1回目

Step 1 - 観測

1回目の観測は、\( z_{1}=49.03m \) です。

高度計の観測誤差の標準偏差 ( \( \sigma \) ) は 5 であるため、分散 ( \( \sigma ^{2} \) ) は 25 になります。そのとき、観測の不確かさは \( r_{1}=25m^{2} \) です。

Step 2 - 更新

カルマンゲインは次のように計算されます。

\[ K_{1}= \frac{p_{1,0}}{p_{1,0}+r_{1}}= \frac{225}{225+25}=0.9 \]

現時点の状態（事後推定値）を推定します。

\[ \hat{x}_{1,1}=~ \hat{x}_{1,0}+ K_{1} \left( z_{1}- \hat{x}_{1,0} \right) =60+0.9 \left( 49.03-60 \right) =50.13m \]

現時点の分散（事後共分散）を更新します。

\[ p_{1,1}=~ \left( 1-K_{1} \right) p_{1,0}= \left( 1-0.9 \right) 225=22.5m^{2} \]

Step 3 - 予測

このシステムは持続予測モデルです。すなわち、ビルの高さは常に一定です。

\[ \hat{x}_{2,1}=\hat{x}_{1,1}= 50.13m \]

また、推定される分散も変化しません。

\[ p_{2,1}= p_{1,1}=22.5m^{2} \]

反復2回目

単位時間後、前の反復での予測推定値 (predicted estimate) は、現時点での反復計算では、事前推定値となります。

\[ \hat{x}_{2,1}=50.13m \]

事前共分散は、前の反復で予測された分散と等しくなります。

\[ p_{2,1}= 22.5m^{2} \]

Step 1 - 観測

2回目の観測値は、\( z_{2}=48.44m \) です。

観測の不確かさは、\( r_{2}=25m^{2} \) です。

Step 2 - 更新

カルマンゲインは次のように計算されます。

\[ K_{2}= \frac{p_{2,1}}{p_{2,1}+r_{2}}= \frac{22.5}{22.5+25}=0.47 \]

事後推定値を計算します。

\[ \hat{x}_{2,2}=~ \hat{x}_{2,1}+ K_{2} \left( z_{2}- x_{2,1} \right) =50.13+0.47 \left( 48.44-50.13 \right) =49.33m \]

事後共分散を計算します。

\[ p_{2,2}=~ \left( 1-K_{2} \right) p_{2,1}= \left( 1-0.47 \right) 22.5=11.84m^{2} \]

Step 3 - 予測

このシステムは持続予測モデルです。すなわち、ビルの高さは常に一定です。

\[ \hat{x}_{3,2}=\hat{x}_{2,2}= 49.33m \]

また、推定される分散も変化しません。

\[ p_{3,2}= p_{2,2}=11.84m^{2} \]

反復3-10回目

以降の反復計算を、次の表にまとめます。

\( n \)	\( z_{n} \)	事後推定値 ( \( K_{n} \) , \( \hat{x}_{n,n} \) , \( p_{n,n} \) )	予測値 ( \( \hat{x}_{n+1,n} \) , \( p_{n+1,n} \) )
3	\( 55.21m \)	\[ K_{3}= \frac{11.84}{11.84+25}=0.32 \] \[ \hat{x}_{3,3}=~ 49.33+0.32 \left( 55.21 -49.33 \right) =51.22m \] \[ p_{3,3}= \left( 1-0.32 \right) 11.84=8.04m^{2} \]	\[ \hat{x}_{4,3}= \hat{x}_{3,3}=51.22m \] \[ p_{4,3}= p_{3,3}=8.04m^{2} \]
4	\( 49.98m \)	\[ K_{4}= \frac{8.04}{8.04+25}=0.24 \] \[ \hat{x}_{4,4}= 51.22+0.24 \left( 49.98 -51.22 \right) =50.92m \] \[ p_{4,4}= \left( 1-0.24 \right) 8.04=6.08m^{2} \]	\[ \hat{x}_{5,4}= \hat{x}_{4,4}=50.92m \] \[ p_{5,4}= p_{4,4}=6.08m^{2} \]
5	\( 50.6m \)	\[ K_{5}= \frac{6.08}{6.08+25}=0.2 \] \[ \hat{x}_{5,5}= 50.92+0.2 \left( 50.6 -50.92 \right) =50.855m \] \[ p_{5,5}= \left( 1-0.2 \right) 6.08=4.89m^{2} \]	\[ \hat{x}_{6,5}= \hat{x}_{5,5}=50.855m \] \[ p_{6,5}= p_{5,5}=4.89m^{2} \]
6	\( 52.61m \)	\[ K_{6}= \frac{4.89}{4.89+25}=0.16 \] \[ \hat{x}_{6,6}=~ 50.855+0.16 \left( 52.61 -50.855 \right) =51.14m \] \[ p_{6,6}= \left( 1-0.16 \right) 4.89=4.09m^{2} \]	\[ \hat{x}_{7,6}= \hat{x}_{6,6}=51.14m \] \[ p_{7,6}= p_{6,6}=4.09m^{2} \]
7	\( 45.87m \)	\[ K_{7}= \frac{4.09}{4.09+25}=0.14 \] \[ \hat{x}_{7,7}= 51.14+0.14 \left( 45.87 -51.14 \right) =50.4m \] \[ p_{7,7}= \left( 1-0.14 \right) 4.09=3.52m^{2} \]	\[ \hat{x}_{8,7}= \hat{x}_{7,7}=50.4m \] \[ p_{8,7}= p_{7,7}=3.52m^{2} \]
8	\( 42.64m \)	\[ K_{8}= \frac{3.52}{3.52+25}=0.12 \] \[ \hat{x}_{8,8}= 50.4+0.12 \left( 42.64 -50.4 \right) =49.44m \] \[ p_{8,8}= \left( 1-0.12 \right) 3.52=3.08m^{2} \]	\[ \hat{x}_{9,8}= \hat{x}_{8,8}=49.44m \] \[ p_{9,8}= p_{8,8}=3.08m^{2} \]
9	\( 48.26m \)	\[ K_{9}= \frac{3.08}{3.08+25}=0.11 \] \[ \hat{x}_{9,9}= 49.44 + 0.11 \left( 48.26 - 49.44 \right) = 49.31m \] \[ p_{9,9}= \left( 1-0.11 \right) 3.08=2.74m^{2} \]	\[ \hat{x}_{10,9}= \hat{x}_{9,9} = 49.31m \] \[ p_{10,9}= p_{9,9}=2.74m^{2} \]
10	\( 55.84m \)	\[ K_{10}= \frac{2.74}{2.74+25}=0.1 \] \[ \hat{x}_{10,10}= 49.31 + 0.1 \left( 55.84 - 49.31 \right) = 49.96m \] \[ p_{10,10}= \left( 1-0.1 \right) 2.74=2.47m^{2} \]	\[ \hat{x}_{11,10}= \hat{x}_{10,10}=49.96m \] \[ p_{11,10}= p_{10,10}=2.47m^{2} \]