例題
これは多次元カルマンフィルタの章の最後のパートです。
ここでは2つの数値例を扱います。最初の例では、制御入力なしの6次元カルマンフィルタを設計します。2番目の例では、制御入力を持つ2次元カルマンフィルタを設計します。
例9 – 車両の位置推定
この例では、学んだ内容を使用して多次元カルマンフィルタを実装します。
この例では、車両の位置を \( XY \) 平面上で推定します。
車両には、システムの \( X \) および \( Y \) 座標を観測する位置センサが搭載されています。
ここでは等加速度の運動モデルを仮定します。
状態方程式
最初に、状態方程式を導出します。ご存知の通り、行列表記での状態方程式の一般形は次の通りです。
| \( \boldsymbol{\hat{x}}_{n+1,n} \) | 時間ステップ \( n + 1 \) における予測システムの状態量ベクトル |
| \( \boldsymbol{\hat{x}}_{n,n} \) | 時間ステップ \( n \) における推定システムの状態量ベクトル |
| \( \boldsymbol{u}_{n} \) | 制御変数 |
| \( \boldsymbol{w}_{n}\) | プロセスノイズ |
| \( \boldsymbol{F} \) | 状態遷移行列 |
| \( \boldsymbol{G} \) | 制御行列 |
この例では、制御入力がないため制御変数 \( \boldsymbol{u} \) はありません。
この例の状態方程式は、次のように簡略化できます。
\[ \boldsymbol{\hat{x}}_{n+1,n} = \boldsymbol{F\hat{x}}_{n,n} \]
システムの状態量 \( \boldsymbol{x}_{n} \) は次のように定義されます。
\[ \boldsymbol{x}_{n}= \left[ \begin{matrix} x_{n}\\ \dot{x}_{n}\\ \ddot{x}_{n}\\ y_{n}\\ \dot{y}_{n}\\ \ddot{y}_{n}\\ \end{matrix} \right] \]
時刻 \( n + 1 \) における車両の状態の予測値は、次の連立方程式で表せます。
\[ \begin{cases} \hat{x}_{n+1,n} = \hat{x}_{n,n} + \hat{\dot{x}}_{n,n} \Delta t+ \frac{1}{2}\hat{\ddot{x}}_{n,n}\Delta t^{2}\\ \hat{\dot{x}}_{n+1,n} = \hat{\dot{x}}_{n,n} + \hat{\ddot{x}}_{n,n}\Delta t\\ \hat{\ddot{x}}_{n+1,n} = \hat{\ddot{x}}_{n,n}\\ \hat{y}_{n+1,n} = \hat{y}_{n,n} + \hat{\dot{y}}_{n,n} \Delta t+ \frac{1}{2}\hat{\ddot{y}}_{n,n}\Delta t^{2}\\ \hat{\dot{y}}_{n+1,n} = \hat{\dot{y}}_{n,n} + \hat{\ddot{y}}_{n,n}\Delta t\\ \hat{\ddot{y}}_{n+1,n} = \hat{\ddot{y}}_{n,n} \end{cases} \]
行列表記では次のようになります。
\[ \left[ \begin{matrix} \hat{x}_{n+1,n}\\ \hat{\dot{x}}_{n+1,n}\\ \hat{\ddot{x}}_{n+1,n}\\ \hat{y}_{n+1,n}\\ \hat{\dot{y}}_{n+1,n}\\ \hat{\ddot{y}}_{n+1,n}\\ \end{matrix} \right] = \left[ \begin{matrix} 1 & \Delta t & 0.5\Delta t^{2} & 0 & 0 & 0\\ 0 & 1 & \Delta t & 0 & 0 & 0\\ 0 & 0 & 1 & 0 & 0 & 0\\ 0 & 0 & 0 & 1 & \Delta t & 0.5\Delta t^{2}\\ 0 & 0 & 0 & 0 & 1 & \Delta t\\ 0 & 0 & 0 & 0 & 0 & 1\\ \end{matrix} \right] \left[ \begin{matrix} \hat{x}_{n,n}\\ \hat{\dot{x}}_{n,n}\\ \hat{\ddot{x}}_{n,n}\\ \hat{y}_{n,n}\\ \hat{\dot{y}}_{n,n}\\ \hat{\ddot{y}}_{n,n}\\ \end{matrix} \right] \]
共分散方程式
共分散方程式の一般形は次のように表されます。
| \( \boldsymbol{P}_{n,n} \) | 現在の状態推定の共分散行列 |
| \( \boldsymbol{P}_{n+1,n} \) | 次の状態推定(予測)の共分散行列 |
| \( \boldsymbol{F} \) | 「線形動的システムのモデリング」の節で導出した状態遷移行列 |
| \( \boldsymbol{Q} \) | プロセスノイズの共分散行列 |
推定誤差共分散行列は次のように表されます。
\[ \boldsymbol{P} = \left[ \begin{matrix} p_{x} & p_{x\dot{x}} & p_{x\ddot{x}} & p_{xy} & p_{x\dot{y}} & p_{x\ddot{y}} \\ p_{\dot{x}x} & p_{\dot{x}} & p_{\dot{x}\ddot{x}} & p_{\dot{x}y} & p_{\dot{x}\dot{y}} & p_{\dot{x}\ddot{y}} \\ p_{\ddot{x}x} & p_{\ddot{x}\dot{x}} & p_{\ddot{x}} & p_{\ddot{x}y} & p_{\ddot{x}\dot{y}} & p_{\ddot{x}\ddot{y}} \\ p_{yx} & p_{y\dot{x}} & p_{y\ddot{x}} & p_{y} & p_{y\dot{y}} & p_{y\ddot{y}} \\ p_{\dot{y}x} & p_{\dot{y}\dot{x}} & p_{\dot{y}\ddot{x}} & p_{\dot{y}y} & p_{\dot{y}} & p_{\dot{y}\ddot{y}} \\ p_{\ddot{y}x} & p_{\ddot{y}\dot{x}} & p_{\ddot{y}\ddot{x}} & p_{\ddot{y}y} & p_{\ddot{y}\dot{y}} & p_{\ddot{y}} \\ \end{matrix} \right] \]
行列の対角成分は推定値の分散を表します。
- \( p_{x} \) は \( X \) 座標位置の推定誤差の分散
- \( p_{\dot{x}} \) は \( X \) 方向速度の推定誤差の分散
- \( p_{\ddot{x}} \) は \( X \) 方向加速度の推定誤差の分散
- \( p_{y} \) は \( Y \) 座標位置の推定誤差の分散
- \( p_{\dot{y}} \) は \( Y \) 方向速度の推定誤差の分散
- \( p_{\ddot{y}} \) は \( Y \) 方向加速度の推定誤差の分散
- 非対角成分は共分散
\( X \) 軸方向と \( Y \) 軸方向の推定誤差は相関していないと仮定し、それらの相互項を0とします。
結果の分析
次のグラフは、カルマンフィルタによる位置と速度の推定性能を示しています。
左側のグラフは、車両位置の真値、観測値および推定値を比較しています。右側の2つのグラフは、\( x \) 軸の速度と \( y \) 軸の速度の真値、観測値および推定値を比較しています。
図から見て取れるように、カルマンフィルタは車両の追跡に成功しています。
車両の直線運動部分と旋回運動部分を拡大して見てみましょう。
プロット上の円は95%信頼区間の確率楕円を表しています。\( x \) 軸と \( y \) 軸の観測誤差が等しいため、確率楕円は円になります。
車両が直線を移動する間、加速度は一定で0です。しかし、旋回操作の間、車両は円運動による加速度、すなわち向心加速度を受けます。
初歩の物理学で学んだように、向心加速度は \( \alpha = \frac{\Delta V}{R \Delta t} \) で表されます。ここで \( \Delta t \) は時間間隔、\( \Delta V \) はその時間間隔内の速度差、\( R \) は円の半径です。
向心加速度の大きさは一定ですが、その \( x \) 軸と \( y \) 軸への射影は一定ではありません。そのため、\( \ddot{x} \) と \( \ddot{y} \) は一定ではありません。
私たちのカルマンフィルタは等加速度モデル用に設計されていますが、適切に選んだ \( \sigma_{a}^{2} \) パラメータにより、旋回する車両の追跡に成功しています。
例10 – ロケットの高度推定
この例では、ロケットの高度を推定します。ロケットには高度を観測する高度計が搭載されています。高度計に加えて、ロケットには加速度を観測する加速度計も搭載されています。
加速度計はカルマンフィルタへの制御入力として機能します。
ここでは、等加速度運動を仮定します。
加速度計の観測には重力加速度が含まれます。机の上で静止している加速度計は上向きに \( 1g \) を観測し、自由落下中の加速度計はゼロ加速度を観測します。したがって、加速度計の各観測から重力加速度 \( g \) を差し引く必要があります。
時間ステップ \( n \) における加速度計の観測値は次のとおりです。
\[ a_{n} = \ddot{x} - g + \epsilon \]
ここで、
- \( \ddot{x} \) は物体の実際の加速度(物体の位置の2階微分)
- \( g \) は重力加速度定数(\( g = -9.8\frac{m}{s^{2}} \))
- \( \epsilon \) は加速度計の観測誤差
状態方程式
行列表記での状態方程式の一般形は次の通りです。
| \( \boldsymbol{\hat{x}}_{n+1,n} \) | 時間ステップ \( n + 1 \) における予測システムの状態量ベクトル |
| \( \boldsymbol{\hat{x}}_{n,n} \) | 時間ステップ \( n \) における推定システムの状態量ベクトル |
| \( \boldsymbol{u}_{n} \) | 制御変数 |
| \( \boldsymbol{w}_{n} \) | プロセスノイズ |
| \( \boldsymbol{F} \) | 状態遷移行列 |
| \( \boldsymbol{G} \) | 制御行列 |
この例では、加速度計の観測に基づいた制御変数 \( \boldsymbol{u} \) が存在します。
システムの状態量 \( \boldsymbol{x}_{n} \) は次のように定義されます。
\[ \boldsymbol{x}_{n} = \left[ \begin{matrix} x_{n} \\ \dot{x}_{n} \\ \end{matrix} \right] \]
ここで、
- \( x_{n} \) は時刻 \( n \) でのロケットの高度
- \( \dot{x}_{n} \) は時刻 \( n \) でのロケットの速度
状態方程式は次のように表現できます。
\[ \left[ \begin{matrix} \hat{x}_{n+1,n}\\ \hat{\dot{x}}_{n+1,n}\\ \end{matrix} \right] = \left[ \begin{matrix} 1 & \Delta t \\ 0 & 1 \\ \end{matrix} \right] \left[ \begin{matrix} \hat{x}_{n,n}\\ \hat{\dot{x}}_{n,n}\\ \end{matrix} \right] + \left[ \begin{matrix} 0.5 \Delta t^{2} \\ \Delta t \\ \end{matrix} \right] \left( a_{n} + g \right) \]
上の式において、
\[ \boldsymbol{F}= \left[ \begin{matrix} 1 & \Delta t \\ 0 & 1 \\ \end{matrix} \right] \]
\[ \boldsymbol{G}= \left[ \begin{matrix} 0.5 \Delta t^{2} \\ \Delta t \\ \end{matrix} \right] \]
\[ \boldsymbol{u}_{n} = \left( a_{n} + g \right) \]
共分散方程式
共分散方程式の一般形は次の通りです。
| \( \boldsymbol{P}_{n,n} \) | 現在の状態推定の共分散行列 |
| \( \boldsymbol{P}_{n+1,n} \) | 次の状態推定(予測)の共分散行列 |
| \( \boldsymbol{F} \) | 「線形動的システムのモデリング」の節で導出した状態遷移行列 |
| \( \boldsymbol{Q} \) | プロセスノイズの共分散行列 |
行列表記での推定誤差共分散は次の通りです。
\[ \boldsymbol{P} = \left[ \begin{matrix} p_{x} & p_{x\dot{x}} \\ p_{\dot{x}x} & p_{\dot{x}} \\ \end{matrix} \right] \]
行列の対角成分は推定値の分散です。
- \( p_{x} \) は高度の推定値の分散
- \( p_{\dot{x}} \) は速度の推定値の分散
- 非対角成分は共分散
状態遷移行列 \( \boldsymbol{F} \) はすでに導出しました。次に、プロセスノイズの共分散行列 \( \boldsymbol{Q} \) を導出します。
プロセスノイズの共分散行列
ここでは、離散ノイズモデルを仮定します。ノイズは各時刻で異なりますが、サンプリング間隔内では一定です。
等加速度モデルのプロセスノイズの共分散行列は次のように表されます。
\[ \boldsymbol{Q} = \left[ \begin{matrix} \sigma_{x}^{2} & \sigma_{x\dot{x}}^{2} \\ \sigma_{\dot{x}x}^{2} & \sigma_{\dot{x}}^{2} \\ \end{matrix} \right] \]
等加速度モデルの行列 \( \boldsymbol{Q} \) はすでに導出しました。この例における行列 \( \boldsymbol{Q} \) は次の通りです。
\[ \boldsymbol{Q} = \left[ \begin{matrix} \frac{\Delta t^{4}}{4} & \frac{\Delta t^{3}}{2} \\ \frac{\Delta t^{3}}{2} & \Delta t^{2} \\ \end{matrix} \right] \epsilon^{2} \]
ここで、
- \( \Delta t \) は観測間隔
- \( \epsilon^{2} \) は加速度計の観測のランダムな分散
前の例では、プロセスノイズの共分散行列の係数として、システムの加速度のランダムな分散 \( \sigma_{a}^{2} \) を使用しました。しかし、ここではシステムのランダムな加速度を観測する加速度計があります。加速度計の誤差 \( v \) はシステムのランダムな加速度よりもはるかに小さいため、プロセスノイズの共分散行列の係数として \( \epsilon^{2} \) を使用します。
これにより、推定の不確かさが大幅に小さくなります!
これで、この例の共分散方程式を書き表すことができます。
結果の分析
次のグラフは、ロケットの高度の真値、観測値および推定値を比較しています。
カルマンフィルタによる良好な追跡性能と収束が見られます。
次のグラフは、ロケットの速度の真値、観測値および推定値を比較しています。
推定値が真のロケットの速度に収束するまでに約2.5秒かかります。
初めは、推定された高度は観測値の影響を受けます。観測値は非常にノイズが大きいため、推定された高度は真のロケットの高度とよく一致していません。
しかし、カルマンフィルタが収束するにつれて、ノイズの多い観測値の影響は小さくなり、推定された高度は真の高度とよく一致するようになります。
この例には加速度の変化を引き起こす機動はありませんが、もしあった場合には、制御入力(加速度計)が状態方程式を更新することになります。