プロセスノイズの追加
この章では、1次元カルマンフィルタモデルにプロセスノイズを追加します。
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\( ^{\circ}C^{2} \)とします。
- 観測誤差（標準偏差）は0.1度です。
- 観測は5秒毎に行われます。
- 観測点における真の液体温度は、50.005\( ^{o}C \), 49.994\( ^{o}C \), 49.993\( ^{o}C \), 50.001\( ^{o}C \), 50.006\( ^{o}C \), 49.998\( ^{o}C \), 50.021\( ^{o}C \), 50.005\( ^{o}C \), 50\( ^{o}C \), 49.997\( ^{o}C \) です。
- 観測で得られた値は、49.986\( ^{o}C \), 49.963\( ^{o}C \), 50.09\( ^{o}C \), 50.001\( ^{o}C \), 50.018\( ^{o}C \), 50.05\( ^{o}C \), 49.938\( ^{o}C \), 49.858\( ^{o}C \), 49.965\( ^{o}C \), 50.114\( ^{o}C \) です。
次のグラフでは、真の液体温度と観測値を比較しています。
反復開始前
1回目の反復計算の前に、カルマンフィルタの初期化と次時点での状態を予測する必要があります。
初期化
私たちは液体の温度を知らないので、ここでは 60\( ^{o}C \) と推測したとします。
\[ \hat{x}_{0,0}=60^{o}C \]
私たちの推測はとても不正確です。なので、推定誤差の標準偏差 \( \sigma \) の初期値は100とします。推定誤差分散の初期値は、推定誤差の分散 \( \left( \sigma ^{2} \right) \) です。
\[ p_{0,0}=100^{2}=10,000^{\circ}C^{2} \]
この分散はとても大きいです。もし私たちが、より意味のある値で初期化を行えば、カルマンフィルタにおいて、より早く推定値が収束します。
予測
初期値に基づいて、次の状態を推定します。
私たちのモデルは持続予測モデルなので、次時点の予測値は現時点の推定値と等しくなります。
\[ \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.986^{o}C \]
観測誤差は 0.1 ( \( \sigma \) ) であるため、分散 ( \( \sigma ^{2} \) ) は0.01となります。そのとき、観測の分散は次のようになります。
\[ r_{1}= 0.01^{\circ}C^{2} \]
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) =60+0.999999 \left( 49.986-60 \right) =49.986^{o}C \]
事後共分散を計算します。
\[ p_{1,1}=~ \left( 1-K_{1} \right) p_{1,0}= \left( 1-0.999999 \right) 10000.0001=0.01^{\circ}C^{2} \]
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.963^{o}C \]
観測誤差は 0.1 ( \( \sigma \) ) であるため、分散 ( \( \sigma^{2} \) ) は0.01となります。そのとき、観測の分散は次のようになります
\[ r_{2}= 0.01^{\circ}C^{2} \]
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.986+0.5 \left( 49.963-49.986 \right) =49.974^{o}C \]
事後共分散を計算します。
\[ p_{2,2}=~ \left( 1-K_{2} \right) p_{2,1}= \left( 1-0.5 \right) 0.0101=0.005^{\circ}C^{2} \]
Step 3 - 予測
このシステムは持続予測モデルです。すなわち、液体の温度は変化しません。
\[ \hat{x}_{3,2}=\hat{x}_{2,2}= 49.974^{o}C \]
次時点の推定の不確かさ（分散）は次のように予測されます。
\[ p_{3,2}= p_{2,2}+q=0.005+ 0.0001=0.0051^{\circ}C^{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 | \( 50.09^{\circ}C \) | \[ K_{3}= \frac{0.0051}{0.0051+0.01}=0.3388 \] \[ \hat{x}_{3,3}=~ 49.974+0.3388 \left( 50.09-49.974 \right) = 50.016^{\circ}C \] \[ p_{3,3}= \left( 1-0.3388 \right)0.0051 =0.0034^{\circ}C^{2} \] | \[ \hat{x}_{4,3}= \hat{x}_{3,3} = 50.016^{\circ}C \] \[ p_{4,3}= 0.0034+0.0001=0.0035^{\circ}C^{2} \] |
| 4 | \( 50.001^{\circ}C \) | \[ K_{4}= \frac{0.0035}{0.0035+0.01}=0.2586 \] \[ \hat{x}_{4,4}=~ 50.016+0.2586 \left( 50.001-50.016 \right) = 50.012^{\circ}C \] \[ p_{4,4}= \left( 1-0.2586 \right) 0.0035=0.0026^{\circ}C^{2} \] | \[ \hat{x}_{5,4}= \hat{x}_{4,4}=50.012^{\circ}C \] \[ p_{5,4}= 0.0026+0.0001=0.0027^{\circ}C^{2} \] |
| 5 | \( 50.018^{\circ}C \) | \[ K_{5}= \frac{0.0027}{0.0027+0.01}=0.2117 \] \[ \hat{x}_{5,5}= 50.012+0.2117 \left( 50.018-50.012 \right) =50.013^{\circ}C \] \[ p_{5,5}= \left( 1-0.2117 \right) 0.0027=0.0021^{\circ}C^{2} \] | \[ \hat{x}_{6,5}= \hat{x}_{5,5}=50.013^{\circ}C \] \[ p_{6,5}= 0.0021+0.0001=0.0022^{\circ}C^{2} \] |
| 6 | \( 50.05^{\circ}C \) | \[ K_{6}= \frac{0.0022}{0.0022+0.01}=0.1815 \] \[ \hat{x}_{6,6}=~ 50.013+0.1815 \left( 50.05-50.013 \right) = 50.02^{\circ}C \] \[ p_{6,6}= \left( 1-0.1815 \right) 0.0022=0.0018^{\circ}C^{2} \] | \[ \hat{x}_{7,6}= \hat{x}_{6,6}=50.02^{\circ}C \] \[ p_{7,6}= 0.0018+0.0001=0.0019^{\circ}C^{2} \] |
| 7 | \( 49.938^{\circ}C \) | \[ K_{7}= \frac{0.0019}{0.0019+0.01}=0.1607 \] \[ \hat{x}_{7,7}=~ 50.02 + 0.1607 \left( 49.938-50.02 \right) = 50.007^{\circ}C \] \[ p_{7,7}= \left( 1-0.1607 \right) 0.0019=0.0016^{\circ}C^{2} \] | \[ \hat{x}_{8,7}= \hat{x}_{7,7}=50.007^{\circ}C \] \[ p_{8,7}= 0.0016+0.0001=0.0017^{\circ}C^{2} \] |
| 8 | \( 49.858^{\circ}C \) | \[ K_{8}= \frac{0.0017}{0.0017+0.01}=0.1458 \] \[ \hat{x}_{8,8}= 50.007+0.1458 \left( 49.858-50.007 \right) =49.985^{\circ}C \] \[ p_{8,8}= \left( 1-0.1458 \right) 0.0017=0.0015 \] | \[ \hat{x}_{9,8}= \hat{x}_{8,8}=49.985^{\circ}C \] \[ p_{9,8}= 0.0015+0.0001=0.0016^{\circ}C^{2} \] |
| 9 | \( 49.965^{\circ}C \) | \[ K_{9}= \frac{0.0016}{0.0016+0.01}=0.1348 \] \[ \hat{x}_{9,9}=~ 49.985+0.1348 \left( 49.965-49.985 \right) =49.982^{\circ}C \] \[ p_{9,9}= \left( 1-0.1348 \right) 0.0016=0.0014^{\circ}C^{2} \] | \[ \hat{x}_{10,9}= \hat{x}_{9,9}=49.982^{\circ}C \] \[ p_{10,9}= 0.0014+0.0001=0.0015^{\circ}C^{2} \] |
| 10 | \( 50.114^{\circ}C \) | \[ K_{10}= \frac{0.0015}{0.0015+0.01}=0.1265 \] \[ \hat{x}_{10,10}=~ 49.982+0.1265 \left( 50.114 -49.982 \right) =49.999^{\circ}C \] \[ p_{10,10}= \left( 1-0.1265 \right) 0.0015=0.0013^{\circ}C^{2} \] | \[ \hat{x}_{11,10}= \hat{x}_{10,10}=49.999^{\circ}C \] \[ p_{11,10}= 0.0013+0.0001=0.0014^{\circ}C^{2} \] |
結果の分析
次のグラフはカルマンゲインを示しています。
ご覧のとおり、カルマンゲインは徐々に減少しており、そのため KF は収束します。
次のグラフは、真値、測定値、および推定値を比較したものです。信頼区間は 95% です。
信頼区間の計算ガイドラインは こちら にあります。
ご覧のとおり、推定値は真値へと収束しています。しかし、KF の推定不確かさは 95% の信頼水準に対して高すぎます。
例のまとめ
この例では、一次元カルマンフィルタを使用して液体の温度を測定しました。システムの動特性にはランダムなプロセスノイズが含まれていますが、カルマンフィルタは良好な推定値を提供します。
Example 7 – 加熱している液体の温度推定 Ⅰ
前回の例題のように、本例題ではタンクに入っている液体の温度を推定します。ただし、この例題では、システムは持続予測モデルではありません。液体は毎秒0.1 \( ^{o}C \) の割合で加熱されます。
カルマンフィルタのパラメータは前回の例題と似ています。
- 正確なモデルができたと考えて、プロセスノイズの分散 ( q ) を0.0001\( ^{\circ}C^{2} \)とします。
- 観測誤差（標準偏差）は0.1 \( ^{o}C \) です
- 観測は5秒毎に行われます。
- 静的モデルを考えます。
注意：実際のシステムのダイナミクスは、液体が加熱されているため静的ではありませんが、ここではダイナミクスが静的である（つまり、温度が変化しない）と考えてカルマンフィルタを設計します。
- 観測点における真の液体温度は、50.505\( ^{o}C \), 50.994\( ^{o}C \), 51.493\( ^{o}C \), 52.001\( ^{o}C \), 52.506\( ^{o}C \), 52.998\( ^{o}C \), 53.521\( ^{o}C \), 54.005\( ^{o}C \), 54.5\( ^{o}C \), and 54.997\( ^{o}C \) です。
- 観測で得られた値は、50.486\( ^{o}C \), 50.963\( ^{o}C \), 51.597\( ^{o}C \), 52.001\( ^{o}C \), 52.518\( ^{o}C \), 53.05\( ^{o}C \), 53.438\( ^{o}C \), 53.858\( ^{o}C \), 54.523\( ^{o}C \), and 55.114\( ^{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^{\circ}C^{2} \]
この分散はとても大きいです。もし私たちが、より意味のある値で初期化を行えば、カルマンフィルタにおいて、より早く推定値が収束します。
予測
初期値に基づいて、次の状態を推定します。
私たちのモデルは持続予測モデルなので、次時点の予測値は現時点の推定値と等しくなります。
\[ \hat{x}_{1,0}=10^{o}C \]
推定値の不確かさ（分散）は次のようになります。
\[ p_{1,0}= p_{0,0}+q=10000+ 0.0001=10000.0001^{\circ}C^{2} \]
反復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.486^{o}C \) | \[ K_{1}= \frac{10000.0001}{10000.0001+0.01} = 0.999999 \] \[ \hat{x}_{1,1}=~ 10+0.999999 \left( 50.486-10 \right) =50.486^{o}C \] \[ p_{1,1}= \left( 1-0.999999 \right) 10000.0001=0.01^{\circ}C^{2} \] | \[ \hat{x}_{2,1}= \hat{x}_{1,1}=50.486^{o}C \] \[ p_{2,1}= 0.01+0.0001=0.0101^{\circ}C^{2} \] |
| 2 | \( 50.963^{o}C \) | \[ K_{2}= \frac{0.0101}{0.0101+0.01}=0.5025 \] \[ \hat{x}_{2,2}=~ 50.486+0.5025 \left( 50.963-50.486 \right) =50.726^{o}C\] \[ p_{2,2}= \left( 1-0.5025 \right) 0.0101=0.005^{\circ}C^{2} \] | \[ \hat{x}_{3,2}= \hat{x}_{2,2}=50.726^{o}C \] \[ p_{3,2}= 0.005+0.0001=0.0051^{\circ}C^{2} \] |
| 3 | \( 51.597^{o}C \) | \[ K_{3}= \frac{0.0051}{0.0051+0.01}=0.3388 \] \[ \hat{x}_{3,3}=~ 50.726+0.3388 \left( 51.597-50.726 \right) =51.021^{o}C\] \[ p_{3,3}= \left( 1-0.3388 \right) 0.0051=0.0034^{\circ}C^{2} \] | \[ \hat{x}_{4,3}= \hat{x}_{3,3}=51.021^{o}C \] \[ p_{4,3}= 0.0034+0.0001=0.0035^{\circ}C^{2} \] |
| 4 | \( 52.001^{o}C \) | \[ K_{4}= \frac{0.0035}{0.0035+0.01}=0.2586 \] \[ \hat{x}_{4,4}=~ 51.021+0.2586 \left( 52.001-51.021 \right) =51.274^{o}C \] \[ p_{4,4}= \left( 1-0.2586 \right) 0.0035=0.0026^{\circ}C^{2} \] | \[ \hat{x}_{5,4}= \hat{x}_{4,4}=51.274^{o}C \] \[ p_{5,4}= 0.0026+0.0001=0.0027^{\circ}C^{2} \] |
| 5 | \( 52.518^{o}C \) | \[ K_{5}= \frac{0.0027}{0.0027+0.01}=0.2117 \] \[ \hat{x}_{5,5}= 51.274+0.2117 \left( 52.518-51.274 \right) =51.538^{o}C \] \[ p_{5,5}= \left( 1-0.2117 \right) 0.0027=0.0021^{\circ}C^{2} \] | \[ \hat{x}_{6,5}= \hat{x}_{5,5}=51.538^{o}C \] \[ p_{6,5}= 0.0021+0.0001=0.0022^{\circ}C^{2} \] |
| 6 | \( 53.05^{o}C \) | \[ K_{6}= \frac{0.0022}{0.0022+0.01}=0.1815 \] \[ \hat{x}_{6,6}=~ 51.538+0.1815 \left( 53.05-51.538 \right) =51.812^{o}C \] \[ p_{6,6}= \left( 1-0.1815 \right) 0.0022=0.0018^{\circ}C^{2} \] | \[ \hat{x}_{7,6}= \hat{x}_{6,6}=51.812^{o}C \] \[ p_{7,6}= 0.0018+0.0001=0.0019^{\circ}C^{2} \] |
| 7 | \( 53.438^{o}C \) | \[ K_{7}= \frac{0.0019}{0.0019+0.01}=0.1607 \] \[ \hat{x}_{7,7}=~ 51.812+0.1607 \left( 53.438-51.812 \right) =52.0735^{o}C \] \[ p_{7,7}= \left( 1-0.1607 \right) 0.0019=0.0016^{\circ}C^{2} \] | \[ \hat{x}_{8,7}= \hat{x}_{7,7}=52.0735^{o}C \] \[ p_{8,7}= 0.0016+0.0001=0.0017^{\circ}C^{2} \] |
| 8 | \( 53.858^{o}C \) | \[ K_{8}= \frac{0.0017}{0.0017+0.01}=0.1458 \] \[ \hat{x}_{8,8}= 52.0735+0.1458 \left( 53.858-52.0735 \right) =52.334^{o}C \] \[ p_{8,8}= \left( 1-0.1458 \right) 0.0017=0.0015^{\circ}C^{2} \] | \[ \hat{x}_{9,8}= \hat{x}_{8,8}=52.334^{o}C \] \[ p_{9,8}= 0.0015+0.0001=0.0016^{\circ}C^{2} \] |
| 9 | \( 54.523^{o}C \) | \[ K_{9}= \frac{0.0016}{0.0016+0.01}=0.1348 \] \[ \hat{x}_{9,9}=~ 52.334+0.1348 \left( 54.523-52.334 \right) =52.621^{o}C \] \[ p_{9,9}= \left( 1-0.1348 \right) 0.0016=0.0014^{\circ}C^{2} \] | \[ \hat{x}_{10,9}= \hat{x}_{9,9}=52.621^{o}C \] \[ p_{10,9}= 0.0014+0.0001=0.0015^{\circ}C^{2} \] |
| 10 | \( 55.114^{o}C \) | \[ K_{10}= \frac{0.0015}{0.0015+0.01}=0.1265 \] \[ \hat{x}_{10,10}=~ 52.621+0.1265 \left( 55.114 -52.621 \right) =52.936^{o}C \] \[ p_{10,10}= \left( 1-0.1265 \right) 0.0015=0.0013^{\circ}C^{2} \] | \[ \hat{x}_{11,10}= \hat{x}_{10,10}=52.936^{o}C \] \[ p_{11,10}= 0.0013+0.0001=0.0014^{\circ}C^{2} \] |
結果の分析
次のグラフは、真値、測定値、および推定値を比較したものです。
ご覧のとおり、カルマンフィルタは信頼できる推定を提供することに失敗しています。カルマンフィルタの推定には遅れ誤差があります。遅れ誤差はすでに例3で遭遇しました。この例では、一定速度を仮定する \( \alpha - \beta \) フィルタを用いて加速する航空機の位置を推定しました。遅れ誤差は、加速度を仮定する \( \alpha - \beta - \gamma \) フィルタに置き換えた例4で解消されました。
今回のカルマンフィルタの例で遅れ誤差が発生した理由は2つあります：
- ダイナミックモデルがこのケースに適合していないこと。
- 実際の温度変動はもっと大きいにもかかわらず、非常に小さいプロセスノイズ \( \left( q=0.0001^{\circ}C^{2} \right) \) を選んでしまったこと。
遅れ誤差を修正する方法としては、次の2つが考えられます：
- 液体の温度が線形に変化する可能性があるとわかっている場合、その線形変化を考慮した新しいモデルを定義できます。これは例4で行った方法です。この方法が最も望ましい方法です。しかし、温度変化がモデル化できない場合、この方法ではカルマンフィルタの性能は向上しません。
- 一方で、モデルが適切に定義されていないため、プロセスモデルの信頼性を調整するためにプロセスノイズ \( q \) を大きくすることができます。詳細は次の例を参照してください。
もう一つの問題は、推定の不確かさが低すぎることです。KFは正確な推定値を提供できなかっただけでなく、自分の推定値に過度の自信を持っています。これは悪いKF設計の典型例です。
例のまとめ
この例では、一定の動的モデルを持つ一次元カルマンフィルタを使用して、加熱中の液体の温度を測定しました。カルマンフィルタの推定で遅れ誤差が観察されました。誤った動的モデルとプロセスモデルの定義が遅れ誤差の原因です。
適切な動的モデルまたはプロセスモデルの定義によって、遅れ誤差を修正できます。
Example 8 – 加熱している液体の温度推定 Ⅱ
この例題は前回の例題と似ていますが、1箇所だけ変更します。それは、我々のシステムモデルは十分に定義されていないために、プロセスノイズの分散 \( \left( q \right) \) を0.0001\(^{\circ}C^{2}\)から0.15\(^{\circ}C^{2}\)に増やすことです。
反復開始前
1回目の反復計算の前に、カルマンフィルタの初期化と次時点での状態を予測する必要があります。
初期化
初期化の処理は前回の例題と同様に行います。
私たちは液体の温度を知らないので、ここでは 10 \( ^{o}C \) と推測したとします。
\[ \hat{x}_{0,0}=10^{o}C \]
私たちの推測はとても不正確です。なので、推定誤差の標準偏差 \( \sigma \) の初期値は100とします。推定誤差分散の初期値は、推定誤差の分散 \( \left( \sigma ^{2} \right) \) です。
\[ p_{0,0}=100^{2}=10,000^{\circ}C^{2} \]
この分散はとても大きいです。もし私たちが、より意味のある値で初期化を行えば、カルマンフィルタにおいて、より早く推定値が収束します。
予測
初期値に基づいて、次の状態を推定します。
私たちのモデルは持続予測モデルなので、次時点の予測値は現時点の推定値と等しくなります。
\[ \hat{x}_{1,0}=10^{o}C \]
推定値の不確かさ（分散）は次のようになります。
\[ p_{1,0}= p_{0,0}+q=10000+ 0.15=10000.15^{\circ}C^{2} \]
反復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.486^{o}C \) | \[ K_{1}= \frac{10000.15}{10000.15+0.01} = 0.999999 \] \[ \hat{x}_{1,1}=~ 10+0.999999 \left( 50.486-10 \right) =50.486^{o}C \] \[ p_{1,1}= \left( 1-0.999999 \right)10000.15=0.01^{\circ}C^{2} \] | \[ \hat{x}_{2,1}= \hat{x}_{1,1}=50.486^{o}C \] \[ p_{2,1}= 0.01+0.15=0.16^{\circ}C^{2} \] |
| 2 | \( 50.963^{o}C \) | \[ K_{2}= \frac{0.16}{0.16+0.01}=0.9412 \] \[ \hat{x}_{2,2}=~ 50.486+0.9412 \left( 50.963-50.486 \right) =50.934^{o}C\] \[ p_{2,2}= \left( 1-0.9412 \right) 0.16=0.0094^{\circ}C^{2} \] | \[ \hat{x}_{3,2}= \hat{x}_{2,2}=50.934^{o}C \] \[ p_{3,2}= 0.0094+0.15=0.1594^{\circ}C^{2} \] |
| 3 | \( 51.597^{o}C \) | \[ K_{3}= \frac{0.1594}{0.1594+0.01}=0.941 \] \[ \hat{x}_{3,3}=~ 50.934+0.941 \left( 51.597-50.934 \right) =51.556^{o}C\] \[ p_{3,3}= \left( 1-0.941 \right) 0.1594=0.0094^{\circ}C^{2} \] | \[ \hat{x}_{4,3}= \hat{x}_{3,3}=51.556^{o}C \] \[ p_{4,3}= 0.0094+0.15=0.1594^{\circ}C^{2} \] |
| 4 | \( 52.001^{o}C \) | \[ K_{4}= \frac{0.1594}{0.1594+0.01}=0.941 \] \[ \hat{x}_{4,4}=~ 51.556+0.941 \left( 52.001-51.556 \right) =51.975^{o}C \] \[ p_{4,4}= \left( 1-0.941 \right) 0.1594=0.0094^{\circ}C^{2} \] | \[ \hat{x}_{5,4}= \hat{x}_{4,4}=51.975^{o}C \] \[ p_{5,4}= 0.0094+0.15=0.1594^{\circ}C^{2} \] |
| 5 | \( 52.518^{o}C \) | \[ K_{5}= \frac{0.1594}{0.1594+0.01}=0.941 \] \[ \hat{x}_{5,5}= 51.975+0.941 \left( 52.518-51.975 \right) =52.486^{o}C \] \[ p_{5,5}= \left( 1-0.941 \right) 0.1594=0.0094^{\circ}C^{2} \] | \[ \hat{x}_{6,5}= \hat{x}_{5,5}=52.486^{o}C \] \[ p_{6,5}= 0.0094+0.15=0.1594^{\circ}C^{2} \] |
| 6 | \( 53.05^{o}C \) | \[ K_{6}= \frac{0.1594}{0.1594+0.01}=0.941 \] \[ \hat{x}_{6,6}=~ 52.486+0.941 \left( 53.05-52.486 \right) =53.017^{o}C \] \[ p_{6,6}= \left( 1-0.941 \right) 0.1594=0.0094^{\circ}C^{2} \] | \[ \hat{x}_{7,6}= \hat{x}_{6,6}=53.017^{o}C \] \[ p_{7,6}= 0.0094+0.15=0.1594^{\circ}C^{2} \] |
| 7 | \( 53.438^{o}C \) | \[ K_{7}= \frac{0.1594}{0.1594+0.01}=0.941 \] \[ \hat{x}_{7,7}=~ 53.017+0.941 \left( 53.438-53.017 \right) =53.413^{o}C \] \[ p_{7,7}= \left( 1-0.941 \right) 0.1594=0.0094^{\circ}C^{2} \] | \[ \hat{x}_{8,7}= \hat{x}_{7,7}=53.413^{o}C \] \[ p_{8,7}= 0.0094+0.15=0.1594^{\circ}C^{2} \] |
| 8 | \( 53.858^{o}C \) | \[ K_{8}= \frac{0.1594}{0.1594+0.01}=0.941 \] \[ \hat{x}_{8,8}= 53.413+0.941 \left( 53.858-53.413 \right) =53.832^{o}C \] \[ p_{8,8}= \left( 1-0.941 \right) 0.1594=0.0094^{\circ}C^{2} \] | \[ \hat{x}_{9,8}= \hat{x}_{8,8}=53.832^{o}C \] \[ p_{9,8}= 0.0094+0.15=0.1594^{\circ}C^{2} \] |
| 9 | \( 54.523^{o}C \) | \[ K_{9}= \frac{0.1594}{0.1594+0.01}=0.941 \] \[ \hat{x}_{9,9}=~ 53.832+0.941 \left( 54.523-53.832 \right) =54.428^{o}C \] \[ p_{9,9}= \left( 1-0.941 \right) 0.1594=0.0094^{\circ}C^{2} \] | \[ \hat{x}_{10,9}= \hat{x}_{9,9}=54.428^{o}C \] \[ p_{10,9}= 0.0094+0.15=0.1594^{\circ}C^{2} \] |
| 10 | \( 55.114^{o}C \) | \[ K_{10}= \frac{0.1594}{0.1594+0.01}=0.941 \] \[ \hat{x}_{10,10}=~ 54.428+0.941 \left( 55.114 -54.428 \right) =55.074^{o}C \] \[ p_{10,10}= \left( 1-0.941 \right) 0.1594=0.0094^{\circ}C^{2} \] | \[ \hat{x}_{11,10}= \hat{x}_{10,10}=55.074^{o}C \] \[ p_{11,10}= 0.0094+0.15=0.1594^{\circ}C^{2} \] |
以下のグラフは、真値、観測値、推定値を比較したものです。
ご覧のように、推定値は観測値に追従しており、ラグエラーは存在していません。
次のグラフはカルマンゲインの推移です。
プロセスノイズの分散を大きくしたために、観測値の重み係数は推定値の重み係数よりも大きくなります。それゆえ、カルマゲインは0.94と大きい値に収束します。
本例題のまとめ
プロセスノイズの分散を大きく設定することで、ラグエラーを防ぐことができます。しかし、我々のシステムモデルは十分に定義されていないため、観測値とほぼ等しいノイズの多い推定値を得てしまって、カルマンフィルタの目的から逸れてしまいます。
カルマンフィルタの最適な実装方法は、プロセスノイズに少し余裕を持たせた、現実に非常に近いモデルを設計することです。しかし、正確なモデルが常に利用できるとは限りません。例えば、航空機のパイロットが突然の操縦を行い、予測される飛行機の軌道を変更することがあります。この場合、システムのプロセスノイズは増加します。