1次元カルマンフィルタ
この章では、1次元の変数に対するカルマンフィルタについて説明します。この章の主な目的は、複雑で混乱しそうな数学を使わずに、カルマンフィルタの概念を「簡単で直感的な方法」で説明することです。
これから、カルマンフィルタの式に向かって一歩一歩進んでいきましょう。
- はじめに、プロセスノイズのない簡単な例でカルマンフィルタの方程式を導出します。
- 次に、プロセスノイズを加えたモデルを考えます。
プロセスノイズのない1次元カルマンフィルタ
前章でも述べたように、カルマンフィルタは5つの方程式に基づいています。私たちはそのうちの2つをすでに扱いました。
- 状態更新式
- 状態方程式（システムモデル）
この章では、残りの3つの方程式を導出します。
最初の例題（金の延べ棒の重さの測定）を思い出しましょう。私たちは複数の観測を行い、それらを平均して推定値を計算しました（カルマンフィルタの分野では、測定のことを観測と呼ぶことが多いので、今後は観測と呼ぶことにします）。
その結果を次のグラフに示します。
上のグラフでは、真値、推定値、観測値を、各観測回数に対して見ることができます。
観測値（青）と真値（緑）の差が観測誤差です。観測誤差はランダムなので、分散 ( \( \sigma ^{2} \) ) で記述することができます。観測誤差の分散は、使用した秤のマニュアルを見るか、校正手順によって導き出すことができます。観測誤差の分散は、実際には観測の不確かさです。
以下、観測の不確かさを \( r \) と表すことにします。
推定値（赤）と真値（緑）の差が推定誤差です。見ての通り、観測回数を重ねるごとに推定誤差は小さくなり、0に向かって収束していきますが、推定値は真値に向かって収束していきます。推定誤差は分かりませんが、推定の不確かさは推定できます。
以下、推定の不確かさを \( p \) と表すことにします。
では、体重測定のPDF (Probability Density Function、確率密度関数) を見てみましょう。
次のグラフは、金の延べ棒の重さを10回観測したものです。
- 観測値を青の線で示しています。
- 真値を赤の点線で示しています。
- 観測値の確率密度関数は緑の線で示しています。
- 緑色で塗られら領域は、観測値の標準偏差 ( \( \sigma \) ) を示しており、真値がこの範囲にある確率は68.26%です。
見てわかるように、10回中8回は真値に近い値を観測しているので、真値は \( 1 \sigma \) の領域の中にあります。
観測の不確かさ ( \( r \) ) は、観測の分散 ( \( \sigma ^{2} \) ) です。
1次元におけるカルマンゲイン
次に、3つ目の式である、カルマンゲインの式を導きます。ここでは、カルマンゲインの式の直感的な導出を紹介します。数学的な導出は次章以降にします。
カルマンフィルタでは反復計算において、\( \alpha \) -\( \beta \) (-\( \gamma \) ) パラメータが動的に計算されます。これらのパラメータはカルマンゲインと呼ばれ、\( K_{n} \) と表記されます。
カルマンゲインの式は以下の通りです。
| \( p_{n,n-1} \) | 推定の不確かさ |
| \( r_{n} \) | 観測の不確かさ |
カルマンゲインは0から1の間の値を取ります。
\[ 0 \leq K_{n} \leq 1 \]
では、カルマンゲインを用いて、状態更新式を書き換えてみましょう。
ご覧のように、カルマンゲイン \( \left( K_{n} \right) \) は観測値に対する重みであることがわかります。そして、\( \left( 1-K_{n} \right) \) は推定値に対する重みになります。
観測の不確かさが非常に大きく、推定の不確かさが非常に小さい場合、カルマンゲインは0に近くなります。したがって、推定値に大きな重みを与え、観測値には小さな重みを与えます。
一方、観測の不確かさが非常に小さく、推定の不確かさが非常に大きい場合、カルマンゲインは1に近くなります。したがって、推定値に小さな重みを与え、観測値には大きな重みを与えます。
観測の不確かさが推定の不確かさと等しい場合、カルマンゲインは0.5に等しくなります。
カルマンゲインの式は、3つ目のカルマンフィルタの式です。
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次元レーダ追尾問題の場合、予測されるターゲット位置は次のようになります。
すなわち、予測位置は現在の推定位置と現在の推定速度に時間を乗じたものに等しくなります。予測される速度は、現在の推定速度に等しくなります（等速モデルを仮定します）。
推定の不確かさの時間発展は次のようになります。
| \( p^{x} \) | 位置推定の不確かさ |
| \( p^{v} \) | 速度推定の不確かさ |
すなわち、次時点の位置推定の不確かさは、現時点の位置推定の不確かさに、現時点の速度推定の不確かさに時間の2乗を掛けたものを加えた値に等しくなります。次時点の速度推定の不確かさは、現時点の速度推定の不確かさに等しくなります（等速モデルを仮定します）。
最初の例（金の延べ棒の重量測定）では、システムのダイナミクスは一定です。したがって、推定の不確かさの時間発展は次のようになります。
| \( p \) | 金の延べ棒の重量推定の不確かさ |
推定値の不確かさの時間発展式は、5つ目のカルマンフィルタの式です。
カルマンフィルタの式のまとめ
この章では、これらの式を1つのアルゴリズムにまとめます。カルマンフィルターは、\( \alpha \)、\( \beta \)、(\( \gamma \) ) フィルタと同様に、「観測、更新、予測」アルゴリズムを利用しています。
次の図は、アルゴリズムの概略を説明したものです。
フィルタの入力は次の通りです。
- 初期化
- 初期状態量 ( \( \hat{x}_{1,0} \) )
- 初期状態量の不確かさ ( \( p_{1,0} \) )
- 観測
- 観測値 ( \( z_{n} \) )
- 観測値の不確かさ ( \( r_{n} \) )
初期化は1回だけ実行され、次の2つのパラメータが与えられます。
初期化パラメータは、システムやプロセス（例えば、レーダーにおける探索プロセス）、または経験や理論的知識に基づいて推定することが可能です。初期化パラメータが正確ではなくても、カルマンフィルタは実測値に近い値に収束させることができます。
観測は毎ステップにおいて行われます。観測では次の2つのパラメータが与えられます。
カルマンフィルタは、観測値に加えて、観測値の不確かさのパラメータを必要とします。通常、このパラメータは観測に使用した機器のデータシートから提供されるか、観測機器の校正によって得られます。レーダによる観測の不確かさは、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 ) |
カルマンフィルタのブロック線図を次の図で示します。
- Step 0: 初期化
- 初期状態量 ( \( \hat{x}_{1,0} \) )
- 初期状態量の不確かさ ( \( p_{1,0} \) )
- Step 1: 観測
- 観測値 ( \( z_{n} \) )
- 観測値の不確かさ ( \( r_{n} \) )
- Step 2: 状態更新
- 観測値 ( \( z_{n} \) )
- 観測値の不確かさ ( \( r_{n} \) )
- 事前推定値 ( \( \hat{x}_{n,n-1} \) )
- 事前共分散 ( \( p_{n,n-1} \) )
- 事後推定値 ( \( \hat{x}_{n,n} \) )
- 事後共分散 ( \( p_{n,n} \) )
- Step 3: 予測
上記で述べたように、初期化は1回のみ実行され、2つのパラメータが与えられます。
初期化の後に、予測を行います。
観測プロセスでは次の2つのパラメータが与えられます。
状態更新プロセスでは、システムの現在の状態を推定します。
状態更新プロセスの入力は次の通りです。
これらの入力に基づいて、状態更新プロセスはカルマンゲインと次の2つの出力を計算します。
これらの2つのパラメータがカルマンフィルタの出力になります。
予測プロセスは、システムのシステムモデルに基づいて、現時点のシステムの状態量と推定値の不確かさを、次時点の状態へ遷移させるものです。
最初のフィルタリング処理において、初期化された出力は事前推定値・事前共分散として使用されます。
その後のフィルタリング処理では、予測プロセスにおいて得られた予測値が事前推定値・事前共分散として使用されます。
カルマンゲインの直感的な説明
カルマンゲインは新しい推定値を計算する際に、観測値の重みと事前推定値の重みを定義する。
カルマンゲインが大きい場合
推定値の不確かさよりも観測の不確かさが小さい場合、カルマンゲインは大きく（1に近く）なります。これは、新しい推定値が観測値に近くなることを意味します（つまり、観測値を信用します）。次の図は、航空機の追跡アプリケーションにおいて、大きいカルマンゲインが推定値に与える影響を示しています。
カルマンゲインが小さい場合
推定値の不確かさよりも観測の不確かさが大きい場合、カルマンゲインは小さく（0に近く）なります。これは、新しい推定値が事前推定値に近くなることを意味します（つまり、事前推定値を信用します）。次の図は、航空機の追跡アプリケーションにおいて、小さいカルマンゲインが推定値に与える影響を示しています。
今、私たちはカルマンフィルタのアルゴリズムを理解し、最初の数値例に取り組む準備ができました。
Example 5 – ビルの高さの推定
あまり正確でない高度計を用いて、ビルの高さの推定を行います。
少なくとも短時間の観測では、ビルの高さは時間と共に変化しません。
数値例
- 真のビルの高さは 50 m です。
- 高度計の観測誤差（標準偏差）は 5 m です。
- 10回の観測値は、48.54 m, 47.11 m, 55.01 m, 55.15 m, 49.89 m, 40.85 m, 46.72 m, 50.05 m, 51.27 m, 49.95 m です。
反復開始前
初期化
この例題では、初期推定値はビルを眺めることで決定できます。
ビルの高さは、
\[ \hat{x}_{0,0}=60m \]
と推定できます。また、推定値の不確かさの初期値を決定します。人間の推定誤差（標準偏差）は約15 m ( \( \sigma = 15 \) ) と考えられます。従って、分散は \( \sigma ^{2}=225 \) です。
\[ p_{0,0}=225 \]
予測
初期値に基づいて、次の状態を推定します。
このシステムは持続予測モデルです。すなわち、ビルの高さは常に一定です。
\[ \hat{x}_{1,0}=\hat{x}_{0,0}= 60m \]
また、推定される分散も変化しません。
\[ p_{1,0}= p_{0,0}=225 \]
反復1回目
Step 1 - 観測
1回目の観測は、\( z_{1}=48.54m \) です。
高度計の観測誤差の標準偏差 ( \( \sigma \) ) は 5 であるため、分散 ( \( \sigma ^{2} \) ) は 25 になります。そのとき、観測の不確かさは \( r_{1}=25 \) です。
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( 48.54-60 \right) =49.69m \]
現時点の分散（事後共分散）を更新します。
\[ p_{1,1}=~ \left( 1-K_{1} \right) p_{1,0}= \left( 1-0.9 \right) 225=22.5 \]
Step 3 - 予測
このシステムは持続予測モデルです。すなわち、ビルの高さは常に一定です。
\[ \hat{x}_{2,1}=\hat{x}_{1,1}= 49.69m \]
また、推定される分散も変化しません。
\[ p_{2,1}= p_{1,1}=22.5 \]
反復2回目
単位時間後、前の反復での予測推定値 (predicted estimate) は、現時点での反復計算では、事前推定値となります。
\[ \hat{x}_{2,1}=49.69m \]
事前共分散は、前の反復で予測された分散と等しくなります。
\[ p_{2,1}= 22.5 \]
Step 1 - 観測
2回目の観測値は、\( z_{2}=47.11m \) です。
観測の不確かさは、\( r_{2}=25 \) です。
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) =49.69+0.47 \left( 47.11-49.69 \right) =48.47m \]
事後共分散を計算します。
\[ p_{2,2}=~ \left( 1-K_{2} \right) p_{2,1}= \left( 1-0.47 \right) 22.5=11.84 \]
Step 3 - 予測
このシステムは持続予測モデルです。すなわち、ビルの高さは常に一定です。
\[ \hat{x}_{3,2}=\hat{x}_{2,2}= 48.47m \]
また、推定される分散も変化しません。
\[ p_{3,2}= p_{2,2}=11.84 \]
反復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.01m \) | \[ K_{3}= \frac{11.84}{11.84+25}=0.32 \] \[ \hat{x}_{3,3}=~ 48.47+0.32 \left( 55.01 -48.47 \right) =50.57m \] \[ p_{3,3}= \left( 1-0.32 \right) 11.84=8.04 \] | \[ \hat{x}_{4,3}= \hat{x}_{3,3}=50.57m \] \[ p_{4,3}= p_{3,3}=8.04 \] |
| 4 | \( 55.15m \) | \[ K_{4}= \frac{8.04}{8.04+25}=0.24 \] \[ \hat{x}_{4,4}=~ 50.57+0.24 \left( 55.15 -50.57 \right) =51.68m \] \[ p_{4,4}= \left( 1-0.24 \right) 8.04=6.08 \] | \[ \hat{x}_{5,4}= \hat{x}_{4,4}=51.68m \] \[ p_{5,4}= p_{4,4}=6.08 \] |
| 5 | \( 49.89m \) | \[ K_{5}= \frac{6.08}{6.08+25}=0.2 \] \[ \hat{x}_{5,5}= 51.68+0.2 \left( 49.89 -51.68 \right) =51.33m \] \[ p_{5,5}= \left( 1-0.2 \right) 6.08=4.89 \] | \[ \hat{x}_{6,5}= \hat{x}_{5,5}=51.33m \] \[ p_{6,5}= p_{5,5}=4.89 \] |
| 6 | \( 40.85m \) | \[ K_{6}= \frac{4.89}{4.89+25}=0.16 \] \[ \hat{x}_{6,6}=~ 51.33+0.16 \left( 40.85 -51.33 \right) =49.62m \] \[ p_{6,6}= \left( 1-0.16 \right) 4.89=4.09 \] | \[ \hat{x}_{7,6}= \hat{x}_{6,6}=49.62m \] \[ p_{7,6}= p_{6,6}=4.09 \] |
| 7 | \( 46.72m \) | \[ K_{7}= \frac{4.09}{4.09+25}=0.14 \] \[ \hat{x}_{7,7}=~ 49.62+0.14 \left( 46.72 -49.62 \right) =49.21m \] \[ p_{7,7}= \left( 1-0.14 \right) 4.09=3.52 \] | \[ \hat{x}_{8,7}= \hat{x}_{7,7}=49.21m \] \[ p_{8,7}= p_{7,7}=3.52 \] |
| 8 | \( 50.05m \) | \[ K_{8}= \frac{3.52}{3.52+25}=0.12 \] \[ \hat{x}_{8,8}= 49.21+0.12 \left( 50.05 -49.21 \right) =49.31m \] \[ p_{8,8}= \left( 1-0.12 \right) 3.52=3.08 \] | \[ \hat{x}_{9,8}= \hat{x}_{8,8}=49.31m \] \[ p_{9,8}= p_{8,8}=3.08 \] |
| 9 | \( 51.27m \) | \[ K_{9}= \frac{3.08}{3.08+25}=0.11 \] \[ \hat{x}_{9,9}=~ 49.31+0.11 \left( 51.27 -49.31 \right) =49.53m \] \[ p_{9,9}= \left( 1-0.11 \right) 3.08=2.74 \] | \[ \hat{x}_{10,9}= \hat{x}_{9,9}=49.53m \] \[ p_{10,9}= p_{9,9}=2.74 \] |
| 10 | \( 49.95m \) | \[ K_{10}= \frac{2.74}{2.74+25}=0.1 \] \[ \hat{x}_{10,10}=~ 49.53+0.1 \left( 49.95 -49.53 \right) =49.57m \] \[ p_{10,10}= \left( 1-0.1 \right) 2.74=2.47 \] | \[ \hat{x}_{11,10}= \hat{x}_{10,10}=49.57m \] \[ p_{11,10}= p_{10,10}=2.47 \] |
以下のグラフは、真値、観測値、推定値を比較したものです。
ご覧のように、7回目の観測後には、推定値は約49.5 mに収束しています。
次のグラフは、観測値の分散と推定値の分散を比較したものです。
1回目の反復計算では、推定値の分散は観測値の分散に近いですが、すぐに減少します。10回の観測後、推定値の分散 ( \( \sigma ^{2} \) ) は2.47であり、推定誤差の標準偏差は \( \sigma = \sqrt[]{2.47}=1.57 m \) です。
したがって、ビルの高さの推定値は、\( 49.57 \pm 1.57m \) と求まります。
次のグラフはカルマンゲインの推移です。
ご覧のように、カルマンゲインは減少しており、したがって、反復ごとに観測値の重み係数は減少します。
本例題のまとめ
この例題では、1次元カルマンフィルタを持ちて、ビルの高さの推定を行いました。\( \alpha -\beta -(\gamma) \) フィルタとは異なり、カルマンゲインは動的であり、観測機器の精度によって変化します。
カルマンフィルタで使用される初期値は、正確ではありせん。したがって、最初の反復では、状態更新式における観測値の重み係数や、推定値の分散は大きくなります。
しかし、これらは反復計算を繰り返すことで、徐々に減少します。
カルマンフィルタでは、状態の推定値とその分散を得ることができます。。
1次元カルマンフィルタの完全体
ここでは、分散の遷移方程式に、プロセスノイズの分散を追加します。
プロセスノイズ
現実世界では、システムのシステムモデルには不確かさが存在します。例えば、抵抗の抵抗値を推定したいとしましょう。このとき、私たちは測定において抵抗値は変化しないとする、持続予測モデルを仮定するでしょう。しかし、実際は環境の温度変化によって、抵抗値が少し変化してしまいます。他にも、レーダで弾道ミサイルを追跡するとき、ターゲットの加速度はシステムモデルの不確かさです。航空機においては、航空機の操縦による旋回運動などにより、不確かさはより大きくなります。
一方で、GPS受信機を用いて静止している物体の位置を推定するとき、物体は動かないのでシステムモデルの不確かさはゼロです。システムモデル（システム）の不確かさはプロセスノイズと呼ばれます。また文献によっては、システムノイズ、システム雑音、プラントノイズなどと呼ばれます。
前回の例題では、ビルの高さを推定しました。ビルの高さは変化しないので、プロセスノイズは考慮しませんでした。
プロセスノイズの分散は \( q \) で表記されます。
共分散の遷移方程式は、プロセスノイズの分散に関する項を含める必要があります。
持続予測モデルの共分散の遷移方程式は次のように表されます。
\[ p_{n+1,n}= p_{n,n}+ q_{n} \]
以下はプロセスノイズを考慮した、1次元カルマンフィルタの式です。
| 方程式 | 名前 | 文献等で使用される名前 |
|---|---|---|
| \( \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} + q_{n} \) (持続予測モデル) |
共分散遷移方程式 | 予測共分散方程式 ( Predictor Covariance Equation ) |
Example 6 – 液体の温度推定
この例題では、タンクに入った液体の温度を推定します。
液体の温度の状態は一定であると仮定します。しかし、真の液体の温度は微小変動します。私たちは、システムのダイナミクスを次のように表現できます。
\[ x_{n}=T+ w_{n} \]
ここで、
\( T \) は一定温度
\( w_{n} \) は分散 \( q \) を持つランダムプロセスノイズ
The numerical example
- 真の温度は50度としましょう。
- 正確なモデルができたと考えて、プロセスノイズの分散 ( \( q \) ) を0.0001とします。
- 観測誤差（標準偏差）は0.1度です。
- 観測は5秒毎に行われます。
- 観測点における真の液体温度は、49.979\( ^{o}C \), 50.025\( ^{o}C \), 50\( ^{o}C \), 50.003\( ^{o}C \), 49.994\( ^{o}C \), 50.002\( ^{o}C \), 49.999\( ^{o}C \), 50.006\( ^{o}C \), 49.998\( ^{o}C \), 49.991\( ^{o}C \) です。
- 観測で得られた値は、49.95\( ^{o}C \), 49.967\( ^{o}C \), 50.1\( ^{o}C \), 50.106\( ^{o}C \), 49.992\( ^{o}C \), 49.819\( ^{o}C \), 49.933\( ^{o}C \), 50.007\( ^{o}C \), 50.023\( ^{o}C \), 49.99\( ^{o}C \) です。
次のグラフでは、真の液体温度と観測値を比較しています。
反復開始前
1回目の反復計算の前に、カルマンフィルタの初期化と次時点での状態を予測する必要があります。
初期化
私たちは液体の温度を知らないので、ここでは 10\( ^{o}C \) と推測したとします。
\[ \hat{x}_{0,0}=10^{o}C \]
私たちの推測はとても不正確です。なので、推定誤差の標準偏差 \( \sigma \) の初期値は100とします。推定誤差分散の初期値は、推定誤差の分散 \( \left( \sigma ^{2} \right) \) です。
\[ p_{0,0}=100^{2}=10,000 \]
この分散はとても大きいです。もし私たちが、より意味のある値で初期化を行えば、カルマンフィルタにおいて、より早く推定値が収束します。
予測
初期値に基づいて、次の状態を推定します。
私たちのモデルは持続予測モデルなので、次時点の予測値は現時点の推定値と等しくなります。
\[ \hat{x}_{1,0}=10^{o}C \]
推定値の不確かさ（分散）は次のようになります。
\[ p_{1,0}= p_{0,0}+q=10000+ 0.0001=10000.0001 \]
反復1回目
Step 1 - 観測
1回目の観測値は次の値でした。
\[ z_{1}=~ 49.95^{o}C \]
観測誤差は 0.1 ( \( \sigma \) ) であるため、分散 ( \( \sigma ^{2} \) ) は0.01となります。そのとき、観測の分散は次のようになります。
\[ r_{1}= 0.01 \]
Step 2 - 更新
カルマンゲインを計算します。
\[ K_{1}= \frac{p_{1,0}}{p_{1,0}+r_{1}}= \frac{10000.0001}{10000.0001+0.01} = 0.999999 \]
カルマンゲインはおよそ1です。すなわち、推定誤差は観測誤差に比べて遥かに大きいことを示しています。したがって、推定値の重み係数はほぼ0、つまり推定値はほとんど無視されます。一方、観測値の重み係数はほぼ1です。
現時点の状態（事後推定値）を推定します。
\[ \hat{x}_{1,1}=~ \hat{x}_{1,0}+ K_{1} \left( z_{1}- \hat{x}_{1,0} \right) =10+0.999999 \left( 49.95-10 \right) =49.95^{o}C \]
事後共分散を計算します。
\[ p_{1,1}=~ \left( 1-K_{1} \right) p_{1,0}= \left( 1-0.999999 \right) 10000.0001=0.01 \]
Step 3 - 予測
このシステムは持続予測モデルです。すなわち、液体の温度は変化しません。
\[ \hat{x}_{2,1}=\hat{x}_{1,1}= 49.95^{o}C \]
次時点の推定の不確かさ（分散）は次のように予測されます。
\[ p_{2,1}= p_{1,1}+q=0.01+ 0.0001=0.0101 \]
反復2回目
Step 1 - 観測
2回目の観測値は次の値でした。
\[ z_{2}=~ 49.967^{o}C \]
観測誤差は 0.1 ( \( \sigma \) ) であるため、分散 ( \( \sigma^{2} \) ) は0.01となります。そのとき、観測の分散は次のようになります
\[ r_{2}= 0.01 \]
Step 2 - 更新
カルマンゲインを計算します。
\[ K_{2}= \frac{p_{2,1}}{p_{2,1}+r_{2}}= \frac{0.0101}{0.0101+0.01} = 0.5 \]
カルマンゲインは0.5です。すなわち、推定値の重み係数と観測値の重み係数は等しくなります。
事後推定値を計算します。
\[ \hat{x}_{2,2}=~ \hat{x}_{2,1}+ K_{2} \left( z_{2}- \hat{x}_{2,1} \right) =49.95+0.5 \left( 49.967-49.95 \right) =49.959^{o}C \]
事後共分散を計算します。
\[ p_{2,2}=~ \left( 1-K_{2} \right) p_{2,1}= \left( 1-0.5 \right) 0.0101=0.005 \]
Step 3 - 予測
このシステムは持続予測モデルです。すなわち、液体の温度は変化しません。
\[ \hat{x}_{3,2}=\hat{x}_{2,2}= 49.959^{o}C \]
次時点の推定の不確かさ（分散）は次のように予測されます。
\[ p_{3,2}= p_{2,2}+q=0.005+ 0.0001=0.0051 \]
反復3-10回目
以降の反復計算を、次の表にまとめます。
| \( n \) | \( z_{n} \) | 事後推定値 ( \( K_{n} \) , \( \hat{x}_{n,n} \) , \( p_{n,n} \) ) | 予測値 ( \( \hat{x}_{n+1,n} \) , \( p_{n+1,n} \) ) |
|---|---|---|---|
| 3 | \( 50.1^{o}C \) | \[ K_{3}= \frac{0.0051}{0.0051+0.01}=0.3388 \] \[ \hat{x}_{3,3}=~ 49.959+0.3388 \left( 50.1-49.959 \right) =50.007^{o}C \] \[ p_{3,3}= \left( 1-0.3388 \right)0.0051 =0.0034 \] | \[ \hat{x}_{4,3}= \hat{x}_{3,3}=50.007^{o}C \] \[ p_{4,3}= 0.0034+0.0001=0.0035 \] |
| 4 | \( 50.106^{o}C \) | \[ K_{4}= \frac{0.0035}{0.0035+0.01}=0.2586 \] \[ \hat{x}_{4,4}=~ 50.007+0.2586 \left( 50.106-50.007 \right) =50.032^{o}C \] \[ p_{4,4}= \left( 1-0.2586 \right) 0.0035=0.0026 \] | \[ \hat{x}_{5,4}= \hat{x}_{4,4}=50.032^{o}C \] \[ p_{5,4}= 0.0026+0.0001=0.0027 \] |
| 5 | \( 49.992^{o}C \) | \[ K_{5}= \frac{0.0027}{0.0027+0.01}=0.2117 \] \[ \hat{x}_{5,5}= 50.032+0.2117 \left( 49.992-50.032 \right) =50.023^{o}C \] \[ p_{5,5}= \left( 1-0.2117 \right) 0.0027=0.0021 \] | \[ \hat{x}_{6,5}= \hat{x}_{5,5}=50.023^{o}C \] \[ p_{6,5}= 0.0021+0.0001=0.0022 \] |
| 6 | \( 49.819^{o}C \) | \[ K_{6}= \frac{0.0022}{0.0022+0.01}=0.1815 \] \[ \hat{x}_{6,6}=~ 50.023+0.1815 \left( 49.819-50.023 \right) =49.987^{o}C \] \[ p_{6,6}= \left( 1-0.1815 \right) 0.0022=0.0018 \] | \[ \hat{x}_{7,6}= \hat{x}_{6,6}=49.987^{o}C \] \[ p_{7,6}= 0.0018+0.0001=0.0019 \] |
| 7 | \( 49.933^{o}C \) | \[ K_{7}= \frac{0.0019}{0.0019+0.01}=0.1607 \] \[ \hat{x}_{7,7}=~ 49.987+0.1607 \left( 49.933-49.987 \right) =49.978^{o}C \] \[ p_{7,7}= \left( 1-0.1607 \right) 0.0019=0.0016 \] | \[ \hat{x}_{8,7}= \hat{x}_{7,7}=49.978^{o}C \] \[ p_{8,7}= 0.0016+0.0001=0.0017 \] |
| 8 | \( 50.007^{o}C \) | \[ K_{8}= \frac{0.0017}{0.0017+0.01}=0.1458 \] \[ \hat{x}_{8,8}= 49.978+0.1458 \left( 50.007-49.978 \right) =49.983^{o}C \] \[ p_{8,8}= \left( 1-0.1458 \right) 0.0017=0.0015 \] | \[ \hat{x}_{9,8}= \hat{x}_{8,8}=49.983^{o}C \] \[ p_{9,8}= 0.0015+0.0001=0.0016 \] |
| 9 | \( 50.023^{o}C \) | \[ K_{9}= \frac{0.0016}{0.0016+0.01}=0.1348 \] \[ \hat{x}_{9,9}=~ 49.983+0.1348 \left( 50.023-49.983 \right) =49.988^{o}C \] \[ p_{9,9}= \left( 1-0.1348 \right) 0.0016=0.0014 \] | \[ \hat{x}_{10,9}= \hat{x}_{9,9}=49.988^{o}C \] \[ p_{10,9}= 0.0014+0.0001=0.0015 \] |
| 10 | \( 49.99^{o}C \) | \[ K_{10}= \frac{0.0015}{0.0015+0.01}=0.1265 \] \[ \hat{x}_{10,10}=~ 49.988+0.1265 \left( 49.99 -49.988 \right) =49.988^{o}C \] \[ p_{10,10}= \left( 1-0.1265 \right) 0.0015=0.0013 \] | \[ \hat{x}_{11,10}= \hat{x}_{10,10}=49.988^{o}C \] \[ p_{11,10}= 0.0013+0.0001=0.0014 \] |
以下のグラフは、真値、観測値、推定値を比較したものです。
ご覧のように、推定値は、真値に向かって収束しています。
次のグラフは推定値の不確かさ（分散）を示しています。
推定値の不確かさは急激に減少しています。10回の観測後では、推定値の不確かさ ( \( \sigma ^{2} \) ) は0.0013です。すなわち、推定誤差の標準偏差は \( \sigma = \sqrt[]{0.0013}=0.036^{o}C \) です。
したがって、液体の温度の推定値は \( 49.988 \pm 0.036_{ }^{o}C \) となります。
次のグラフはカルマンゲインの推移です。
ご覧のように、カルマンゲインは減少しており、したがって、反復ごとに観測値の重み係数は減少します。
本例題のまとめ
この例題では、1次元カルマンフィルタを持ちて、液体の温度の推定を行いました。システムにはランダムなプロセスノイズが含まれていましたが、カルマンフィルタを用いて良好な推定値を得ることができました。
Example 7 – 加熱している液体の温度推定 Ⅰ
前回の例題のように、本例題ではタンクに入っている液体の温度を推定します。ただし、この例題では、システムは持続予測モデルではありません。液体は毎秒0.1 \( ^{o}C \) の割合で加熱されます。
カルマンフィルタのパラメータは前回の例題と似ています。
- 正確なモデルができたと考えて、プロセスノイズの分散 ( q ) を0.0001とします。
- 観測誤差（標準偏差）は0.1 \( ^{o}C \) です
- 観測は5秒毎に行われます。
- 静的モデルを考えます。
注意：実際のシステムのダイナミクスは、液体が加熱されているため静的ではありませんが、ここではダイナミクスが静的である（つまり、温度が変化しない）と考えてカルマンフィルタを設計します。
- 観測点における真の液体温度は、50.479\( ^{o}C \), 51.025\( ^{o}C \), 51.5\( ^{o}C \), 52.003\( ^{o}C \), 52.494\( ^{o}C \), 53.002\( ^{o}C \), 53.499\( ^{o}C \), 54.006\( ^{o}C \), 54.498\( ^{o}C \), and 54.991\( ^{o}C \) です。
- 観測で得られた値は、50.45\( ^{o}C \), 50.967\( ^{o}C \), 51.6\( ^{o}C \), 52.106\( ^{o}C \), 52.492\( ^{o}C \), 52.819\( ^{o}C \), 53.433\( ^{o}C \), 54.007\( ^{o}C \), 54.523\( ^{o}C \), and 54.99\( ^{o}C \) です。
次のグラフでは、真の液体温度と観測値を比較しています。
反復開始前
前回の例題と同様にして、初期化を行います。
1回目の反復計算の前に、カルマンフィルタの初期化と次時点での状態を予測する必要があります。
初期化
私たちは液体の温度を知らないので、ここでは 10 \( ^{o}C \) と推測したとします。
\[ \hat{x}_{0,0}=10^{o}C \]
私たちの推測はとても不正確です。なので、推定誤差の標準偏差 \( \sigma \) の初期値は100とします。推定誤差分散の初期値は、推定誤差の分散 \( \left( \sigma ^{2} \right) \) です。
\[ p_{0,0}=100^{2}=10,000 \]
この分散はとても大きいです。もし私たちが、より意味のある値で初期化を行えば、カルマンフィルタにおいて、より早く推定値が収束します。
予測
初期値に基づいて、次の状態を推定します。
私たちのモデルは持続予測モデルなので、次時点の予測値は現時点の推定値と等しくなります。
\[ \hat{x}_{1,0}=10^{o}C \]
推定値の不確かさ（分散）は次のようになります。
\[ p_{1,0}= p_{0,0}+q=10000+ 0.0001=10000.0001 \]
反復1-10回目
各反復計算の結果を次の表にまとめます。
| \( n \) | \( z_{n} \) | 事後推定値 ( \( K_{n} \) , \( \hat{x}_{n,n} \) , \( p_{n,n} \) ) | 予測値 ( \( \hat{x}_{n+1,n} \) , \( p_{n+1,n} \) ) |
|---|---|---|---|
| 1 | \( 50.45^{o}C \) | \[ K_{1}= \frac{10000.0001}{10000.0001+0.01} = 0.999999 \] \[ \hat{x}_{1,1}=~ 10+0.999999 \left( 50.45-10 \right) =50.45^{o}C \] \[ p_{1,1}= \left( 1-0.999999 \right) 10000.0001=0.01 \] | \[ \hat{x}_{2,1}= \hat{x}_{1,1}=50.45^{o}C \] \[ p_{2,1}= 0.01+0.0001=0.0101 \] |
| 2 | \( 50.967^{o}C \) | \[ K_{2}= \frac{0.0101}{0.0101+0.01}=0.5025 \] \[ \hat{x}_{2,2}=~ 50.45+0.5025 \left( 50.967-50.45 \right) =50.71^{o}C\] \[ p_{2,2}= \left( 1-0.5025 \right) 0.0101=0.005 \] | \[ \hat{x}_{3,2}= \hat{x}_{2,2}=50.71^{o}C \] \[ p_{3,2}= 0.005+0.0001=0.0051 \] |
| 3 | \( 51.6^{o}C \) | \[ K_{3}= \frac{0.0051}{0.0051+0.01}=0.3388 \] \[ \hat{x}_{3,3}=~ 50.71+0.3388 \left( 51.6-50.71 \right) =51.011^{o}C\] \[ p_{3,3}= \left( 1-0.3388 \right) 0.0051=0.0034 \] | \[ \hat{x}_{4,3}= \hat{x}_{3,3}=51.011^{o}C \] \[ p_{4,3}= 0.0034+0.0001=0.0035 \] |
| 4 | \( 52.106^{o}C \) | \[ K_{4}= \frac{0.0035}{0.0035+0.01}=0.2586 \] \[ \hat{x}_{4,4}=~ 51.011+0.2586 \left( 52.106-51.011 \right) =51.295^{o}C \] \[ p_{4,4}= \left( 1-0.2586 \right) 0.0035=0.0026 \] | \[ \hat{x}_{5,4}= \hat{x}_{4,4}=51.295^{o}C \] \[ p_{5,4}= 0.0026+0.0001=0.0027 \] |
| 5 | \( 52.492^{o}C \) | \[ K_{5}= \frac{0.0027}{0.0027+0.01}=0.2117 \] \[ \hat{x}_{5,5}= 51.295+0.2117 \left( 52.492-51.295 \right) =51.548^{o}C \] \[ p_{5,5}= \left( 1-0.2117 \right) 0.0027=0.0021 \] | \[ \hat{x}_{6,5}= \hat{x}_{5,5}=51.548^{o}C \] \[ p_{6,5}= 0.0021+0.0001=0.0022 \] |
| 6 | \( 52.819^{o}C \) | \[ K_{6}= \frac{0.0022}{0.0022+0.01}=0.1815 \] \[ \hat{x}_{6,6}=~ 51.548+0.1815 \left( 52.819-51.548 \right) =51.779^{o}C \] \[ p_{6,6}= \left( 1-0.1815 \right) 0.0022=0.0018 \] | \[ \hat{x}_{7,6}= \hat{x}_{6,6}=51.779^{o}C \] \[ p_{7,6}= 0.0018+0.0001=0.0019 \] |
| 7 | \( 53.433^{o}C \) | \[ K_{7}= \frac{0.0019}{0.0019+0.01}=0.1607 \] \[ \hat{x}_{7,7}=~ 51.779+0.1607 \left( 53.433-51.779 \right) =52.045^{o}C \] \[ p_{7,7}= \left( 1-0.1607 \right) 0.0019=0.0016 \] | \[ \hat{x}_{8,7}= \hat{x}_{7,7}=52.045^{o}C \] \[ p_{8,7}= 0.0016+0.0001=0.0017 \] |
| 8 | \( 54.007^{o}C \) | \[ K_{8}= \frac{0.0017}{0.0017+0.01}=0.1458 \] \[ \hat{x}_{8,8}= 52.045+0.1458 \left( 54.007-52.045 \right) =52.331^{o}C \] \[ p_{8,8}= \left( 1-0.1458 \right) 0.0017=0.0015 \] | \[ \hat{x}_{9,8}= \hat{x}_{8,8}=52.331^{o}C \] \[ p_{9,8}= 0.0015+0.0001=0.0016 \] |
| 9 | \( 54.523^{o}C \) | \[ K_{9}= \frac{0.0016}{0.0016+0.01}=0.1348 \] \[ \hat{x}_{9,9}=~ 52.331+0.1348 \left( 54.523-52.331 \right) =52.626^{o}C \] \[ p_{9,9}= \left( 1-0.1348 \right) 0.0016=0.0014 \] | \[ \hat{x}_{10,9}= \hat{x}_{9,9}=52.626^{o}C \] \[ p_{10,9}= 0.0014+0.0001=0.0015 \] |
| 10 | \( 54.99^{o}C \) | \[ K_{10}= \frac{0.0015}{0.0015+0.01}=0.1265 \] \[ \hat{x}_{10,10}=~ 2.626+0.1265 \left( 54.99 -52.626 \right) =52.925^{o}C \] \[ p_{10,10}= \left( 1-0.1265 \right) 0.0015=0.0013 \] | \[ \hat{x}_{11,10}= \hat{x}_{10,10}=52.925^{o}C \] \[ p_{11,10}= 0.0013+0.0001=0.0014 \] |
以下のグラフは、真値、観測値、推定値を比較したものです。
ご覧のように、カルマンフィルタは信頼できる推定値を得ることができていません。なぜなら、このカルマンフィルタの推定値にはラグエラーが存在するからです。振り返ると、Example 3では、加速している航空機の位置を、機体速度が一定であると仮定した \( \alpha - \beta \) フィルタで推定したときに、ラグエラーが発生していました。そのために、Example 4では、\( \alpha - \beta \) フィルタを、加速度を仮定した \( \alpha -\beta -\gamma \) フィルタに置き換えることで、ラグエラーを解消することができました。
このカルマンフィルタの例題でラグエラーが発生した理由は2つあります。
- システムモデルがこのケースに適していない。
- 実際の温度変化は大きいにも関わらず、低いプロセスノイズの値 \( \left( q=0.0001 \right) \) を設定してしまった。
このラグエラーを解消するためには2つの方法があります。
- もし、液体の温度が直線的に変化することが分かっていれば、それを考慮した新しいモデルを定義することができます。これについては例題4で扱いました。これは好ましい方法ですが、温度変化をモデル化できない場合、この方法ではカルマンフィルタの性能は向上しません。
- 一方、今回のモデルは定義が完璧ではないので、プロセスノイズの分散 \( \left( q \right) \) を大きくすることで、モデルの信頼度を調整することができます。詳しくは次の例題をご覧ください。
本例題のまとめ
この例題では、持続予測モデルに1次元カルマンフィルタを用いて、加熱される液液の温度を測定しました。結果から、カルマンフィルタの推定にはラグエラーが存在することが確認されました。このラグエラーは、システムモデルの定義とプロセスモデルの定義が誤っていることが原因です。
このラグエラーは、システムモデルやプロセスモデルを適切に定義することによって修正することができます。
Example 8 – 加熱している液体の温度推定 Ⅱ
この例題は前回の例題と似ていますが、1箇所だけ変更します。それは、我々のシステムモデルは十分に定義されていないために、プロセスノイズの分散 \( \left( q \right) \) を0.0001から0.15に増やすことです。
反復開始前
1回目の反復計算の前に、カルマンフィルタの初期化と次時点での状態を予測する必要があります。
初期化
初期化の処理は前回の例題と同様に行います。
私たちは液体の温度を知らないので、ここでは 10 \( ^{o}C \) と推測したとします。
\[ \hat{x}_{0,0}=10^{o}C \]
私たちの推測はとても不正確です。なので、推定誤差の標準偏差 \( \sigma \) の初期値は100とします。推定誤差分散の初期値は、推定誤差の分散 \( \left( \sigma ^{2} \right) \) です。
\[ p_{0,0}=100^{2}=10,000 \]
この分散はとても大きいです。もし私たちが、より意味のある値で初期化を行えば、カルマンフィルタにおいて、より早く推定値が収束します。
予測
初期値に基づいて、次の状態を推定します。
私たちのモデルは持続予測モデルなので、次時点の予測値は現時点の推定値と等しくなります。
\[ \hat{x}_{1,0}=10^{o}C \]
推定値の不確かさ（分散）は次のようになります。
\[ p_{1,0}= p_{0,0}+q=10000+ 0.15=10000.15 \]
反復1-10回目
各反復計算の結果を次の表にまとめます
| \( n \) | \( z_{n} \) | 事後推定値 ( \( K_{n} \) , \( \hat{x}_{n,n} \) , \( p_{n,n} \) ) | 予測値 ( \( \hat{x}_{n+1,n} \) , \( p_{n+1,n} \) ) |
|---|---|---|---|
| 1 | \( 50.45^{o}C \) | \[ K_{1}= \frac{10000.15}{10000.15+0.01} = 0.999999 \] \[ \hat{x}_{1,1}=~ 10+0.999999 \left( 50.45-10 \right) =50.45^{o}C \] \[ p_{1,1}= \left( 1-0.999999 \right)10000.15=0.01 \] | \[ \hat{x}_{2,1}= \hat{x}_{1,1}=50.45^{o}C \] \[ p_{2,1}= 0.01+0.15=0.16 \] |
| 2 | \( 50.967^{o}C \) | \[ K_{2}= \frac{0.16}{0.16+0.01}=0.9412 \] \[ \hat{x}_{2,2}=~ 50.45+0.9412 \left( 50.967-50.45 \right) =50.94^{o}C\] \[ p_{2,2}= \left( 1-0.9412 \right) 0.16=0.0094 \] | \[ \hat{x}_{3,2}= \hat{x}_{2,2}=50.94^{o}C \] \[ p_{3,2}= 0.0094+0.15=0.1594 \] |
| 3 | \( 51.6^{o}C \) | \[ K_{3}= \frac{0.1594}{0.1594+0.01}=0.941 \] \[ \hat{x}_{3,3}=~ 50.94+0.941 \left( 51.6-50.94 \right) =51.56^{o}C\] \[ p_{3,3}= \left( 1-0.941 \right) 0.1594=0.0094 \] | \[ \hat{x}_{4,3}= \hat{x}_{3,3}=51.56^{o}C \] \[ p_{4,3}= 0.0094+0.15=0.1594 \] |
| 4 | \( 52.106^{o}C \) | \[ K_{4}= \frac{0.1594}{0.1594+0.01}=0.941 \] \[ \hat{x}_{4,4}=~ 51.56+0.941 \left( 52.106-51.56 \right) =52.07^{o}C \] \[ p_{4,4}= \left( 1-0.941 \right) 0.1594=0.0094 \] | \[ \hat{x}_{5,4}= \hat{x}_{4,4}=52.07^{o}C \] \[ p_{5,4}= 0.0094+0.15=0.1594 \] |
| 5 | \( 52.492^{o}C \) | \[ K_{5}= \frac{0.1594}{0.1594+0.01}=0.941 \] \[ \hat{x}_{5,5}= 52.07+0.941 \left( 52.492-52.07 \right) =52.47^{o}C \] \[ p_{5,5}= \left( 1-0.941 \right) 0.1594=0.0094 \] | \[ \hat{x}_{6,5}= \hat{x}_{5,5}=52.47^{o}C \] \[ p_{6,5}= 0.0094+0.15=0.1594 \] |
| 6 | \( 52.819^{o}C \) | \[ K_{6}= \frac{0.1594}{0.1594+0.01}=0.941 \] \[ \hat{x}_{6,6}=~ 52.47+0.941 \left( 52.819-52.47 \right) =52.8^{o}C \] \[ p_{6,6}= \left( 1-0.941 \right) 0.1594=0.0094 \] | \[ \hat{x}_{7,6}= \hat{x}_{6,6}=52.8^{o}C \] \[ p_{7,6}= 0.0094+0.15=0.1594 \] |
| 7 | \( 53.433^{o}C \) | \[ K_{7}= \frac{0.1594}{0.1594+0.01}=0.941 \] \[ \hat{x}_{7,7}=~ 52.8+0.941 \left( 53.433-52.8 \right) =53.4^{o}C \] \[ p_{7,7}= \left( 1-0.941 \right) 0.1594=0.0094 \] | \[ \hat{x}_{8,7}= \hat{x}_{7,7}=53.4^{o}C \] \[ p_{8,7}= 0.0094+0.15=0.1594 \] |
| 8 | \( 54.007^{o}C \) | \[ K_{8}= \frac{0.1594}{0.1594+0.01}=0.941 \] \[ \hat{x}_{8,8}= 53.4+0.941 \left( 54.007-53.4 \right) =53.97^{o}C \] \[ p_{8,8}= \left( 1-0.941 \right) 0.1594=0.0094 \] | \[ \hat{x}_{9,8}= \hat{x}_{8,8}=53.97^{o}C \] \[ p_{9,8}= 0.0094+0.15=0.1594 \] |
| 9 | \( 54.523^{o}C \) | \[ K_{9}= \frac{0.1594}{0.1594+0.01}=0.941 \] \[ \hat{x}_{9,9}=~ 53.97+0.941 \left( 54.523-53.97 \right) =54.49^{o}C \] \[ p_{9,9}= \left( 1-0.941 \right) 0.1594=0.0094 \] | \[ \hat{x}_{10,9}= \hat{x}_{9,9}=54.49^{o}C \] \[ p_{10,9}= 0.0094+0.15=0.1594 \] |
| 10 | \( 54.99^{o}C \) | \[ K_{10}= \frac{0.1594}{0.1594+0.01}=0.941 \] \[ \hat{x}_{10,10}=~ 54.49+0.941 \left( 54.99 -54.49 \right) =54.96^{o}C \] \[ p_{10,10}= \left( 1-0.941 \right) 0.1594=0.0094 \] | \[ \hat{x}_{11,10}= \hat{x}_{10,10}=54.96^{o}C \] \[ p_{11,10}= 0.0094+0.15=0.1594 \] |
以下のグラフは、真値、観測値、推定値を比較したものです。
ご覧のように、推定値は観測値に追従しており、ラグエラーは存在していません。
次のグラフはカルマンゲインの推移です。
プロセスノイズの分散を大きくしたために、観測値の重み係数は推定値の重み係数よりも大きくなります。それゆえ、カルマゲインは0.94と大きい値に収束します。
本例題のまとめ
プロセスノイズの分散を大きく設定することで、ラグエラーを防ぐことができます。しかし、我々のシステムモデルは十分に定義されていないため、観測値とほぼ等しいノイズの多い推定値を得てしまって、カルマンフィルタの目的から逸れてしまいます。
カルマンフィルタの最適な実装方法は、プロセスノイズに少し余裕を持たせた、現実に非常に近いモデルを設計することです。しかし、正確なモデルが常に利用できるとは限りません。例えば、航空機のパイロットが突然の操縦を行い、予測される飛行機の軌道を変更することがあります。この場合、システムのプロセスノイズは増加します。