共分散遷移式

プロセス雑音を考慮しない推定の不確かさ

ここでは、プロセス雑音が存在しない \( (Q=0) \) と仮定します。このとき、

\[ \boldsymbol{P_{n+1,n} = FP_{n,n}F^{T}} \]

となります。この遷移式はとても単純な形をしています。以下では、この方程式を導出します。"背景" の章で、次のような式が登場しました。

\[ COV(\boldsymbol{x}) = E \left( \left( \boldsymbol{x - \mu_{x}} \right) \left( \boldsymbol{x - \mu_{x}} \right)^{T} \right) \]

ここで、ベクトル \( x \) はシステムの状態変数ベクトルです。

したがって、

\[ \boldsymbol{P_{n,n}} = E \left( \left( \boldsymbol{\hat{x}_{n,n} - \mu_{x_{n,n}}} \right) \left( \boldsymbol{\hat{x}_{n,n} - \mu_{x_{n,n}}} \right)^{T} \right) \]

ここで、状態方程式より、

\[ \boldsymbol{\hat{x}_{n+1,n}=F\hat{x}_{n,n}+G\hat{u}_{n,n}} \]

であるから、

\[ \boldsymbol{P_{n+1,n}} = E \left( \left( \boldsymbol{\hat{x}_{n+1,n} - \mu_{x_{n+1,n}}} \right) \left( \boldsymbol{\hat{x}_{n+1,n} - \mu_{x_{n+1,n}}} \right)^{T} \right) \]

\[ = E \left( \left( \boldsymbol{F\hat{x}_{n,n} + G\hat{u}_{n,n} - F\mu_{x_{n,n}} - G\hat{u}_{n,n}} \right) \left( \boldsymbol{F\hat{x}_{n,n} + G\hat{u}_{n,n} - F\mu_{x_{n,n}} - G\hat{u}_{n,n}} \right)^{T} \right) \]

\[ = E \left( \boldsymbol{F} \left( \boldsymbol{\hat{x}_{n,n} - \mu_{x_{n,n}}} \right) \left( \boldsymbol{F} \left( \boldsymbol{\hat{x}_{n,n} - \mu_{x_{n,n}}} \right) \right)^{T} \right) \]

ここで、転置行列の性質 \( \boldsymbol{(AB)^T = B^T A^T} \) を用いて、

\[ = E \left(\boldsymbol{F} \left( \boldsymbol{\hat{x}_{n,n} - \mu_{x_{n,n}}} \right) \left( \boldsymbol{\hat{x}_{n,n} - \mu_{x_{n,n}}} \right)^{T} \boldsymbol{F^{T}} \right) \]

\[ = \boldsymbol{F} E \left( \left( \boldsymbol{\hat{x}_{n,n} - \mu_{x_{n,n}}} \right) \left( \boldsymbol{\hat{x}_{n,n} - \mu_{x_{n,n}}} \right)^{T} \right) \boldsymbol{F^{T}} \]

\[ = \boldsymbol{F P_{n,n} F^{T}} \]

プロセス雑音行列 \( Q \) の構成

既に知っているように、システムの状態方程式は次のように表されます。

\[ \boldsymbol{\hat{x}_{n+1,n}=F\hat{x}_{n,n}+G\hat{u}_{n,n}+w_{n}} \]

ここで、\( w_{n} \) は時間ステップ \( n \) におけるプロセス雑音です。

プロセス雑音とそのカルマンフィルタの性能への影響については、"1次元カルマンフィルタ" のセクションで説明しました。1次元カルマンフィルタでは、プロセス雑音の分散を \( q \) と表します。

多次元カルマンフィルタにおいては、プロセス雑音は、共分散行列 \( \boldsymbol{Q} \) で表されます。

プロセス雑音の分散がカルマンフィルタの性能に決定的な影響を与えることを説明しました。\( q \) が小さすぎるとラグ誤差が発生します（例題7を参照）。また、\( q \) 大きすぎると、カルマンフィルターは測定値に追従し（例題8を参照）、雑音の多い推定値を生成してしまいます。

プロセス雑音は、異なる状態変数間で独立な場合があります。このとき、プロセス雑音の共分散行列 \( \boldsymbol{Q} \) は対角行列になります。

\[ \boldsymbol{Q} = \left[ \begin{matrix} q_{11} & 0 & \cdots & 0 \\ 0 & q_{22} & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \cdots & q_{kk} \\ \end{matrix} \right] \]

プロセス雑音が変数間で独立でないことも考えれます。 等速運動モデルの例を見てみます。このモデルは加速度ゼロを仮定しています（ \(a=0\) ）。しかし、加速度にランダムなばらつき \( \sigma^{2}_{a} \) があるとすると、速度と位置にばらつきが生じます。この場合、プロセス雑音は状態変数間で相関を持ちます（つまり、独立でない）。

プロセス雑音には、次の２つのモデルがあります。

• 離散雑音モデル
• 連続雑音モデル

ここでは、どちらのモデルも扱います。

離散雑音モデル

離散雑音モデルは、雑音が各時間帯では異なりますが、長い時間区間では一定であると仮定しています。

等速運動モデルでは、プロセス雑音行列は次のように表されます。

\[ \boldsymbol{Q} = \left[ \begin{matrix} V(x) & COV(x,v) \\ COV(v,x) & V(v) \\ \end{matrix} \right] \]

位置と速度の分散および共分散を、加速度の分散 \( \sigma^{2}_{a} \) を用いて表します。

分散の公式（"背景" のセクションを参照）を用いて、行列の各要素を求めます。

\[ V(v) = \sigma^{2}_{v} = E\left(v^{2}\right) - \mu_{v}^{2} = E \left( \left( a\Delta t\right)^{2}\right) - \left(\mu_{a}\Delta t\right)^{2} = \Delta t^{2}\left( E\left(a^{2}\right) - \mu_{a}^{2} \right) = \Delta t^{2}\sigma^{2}_{a} \]

\[ V(x) = \sigma^{2}_{x} = E\left(x^{2}\right) - \mu_{x}^{2} = E \left( \left( \frac{1}{2}a\Delta t^{2}\right)^{2}\right) - \left(\frac{1}{2}\mu_{a}\Delta t^{2}\right)^{2} = \frac{\Delta t^{4}}{4}\left( E\left(a^{2}\right) - \mu_{a}^{2} \right) = \frac{\Delta t^{4}}{4}\sigma^{2}_{a} \]

\[ COV(x,v) = COV(v,x) = E\left(xv\right) - \mu_{x}\mu_{v} = E\left( \frac{1}{2}a\Delta t^{2}a\Delta t\right) - \left( \frac{1}{2}\mu_{a}\Delta t^{2}\mu_{a}\Delta t\right) = \frac{\Delta t^{3}}{2}\left( E\left(a^{2}\right) - \mu_{a}^{2} \right) = \frac{\Delta t^{3}}{2}\sigma^{2}_{a} \]

これらの結果を、行列 \( \boldsymbol{Q} \) に代入します。

\[ \boldsymbol{Q} = \sigma^{2}_{a} \left[ \begin{matrix} \frac{\Delta t^{4}}{4} & \frac{\Delta t^{3}}{2} \\ \frac{\Delta t^{3}}{2} & \Delta t^{2} \\ \end{matrix} \right] \]

また、次に行列 \( \boldsymbol{Q} \) をより早く求める方法を紹介します。

システム行列を用いた変換

動的モデルに入力が含まれていない場合、システム行列を用いて加速度の分散 \( \sigma^{2}_{a} \) を動的モデルに射影することができます。

ここで、行列 \( \boldsymbol{Q}_{a} \) を定義します。

\[ \boldsymbol{Q}_{a} = \left[ \begin{matrix} 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 1 \\ \end{matrix} \right] \sigma^{2}_{a} \]

プロセス雑音行列は次のように表されます。

\[ \boldsymbol{Q} = \boldsymbol{F}\boldsymbol{Q}_{a}\boldsymbol{F^{T}} \]

運動モデルによって、システム行列 \( \boldsymbol{F} \) は次のように求められます。

\[ \boldsymbol{F} = \left[ \begin{matrix} 1 & \Delta t & \frac{\Delta t^{2}}{2} \\ 0 & 1 & \Delta t \\ 0 & 0 & 1 \\ \end{matrix} \right] \]

\[ \boldsymbol{Q} = \boldsymbol{F}\boldsymbol{Q}_{a}\boldsymbol{F^{T}} = \]

\[ = \left[ \begin{matrix} 1 & \Delta t & \frac{\Delta t^{2}}{2} \\ 0 & 1 & \Delta t \\ 0 & 0 & 1 \\ \end{matrix} \right] \left[ \begin{matrix} 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 1 \\ \end{matrix} \right] \left[ \begin{matrix} 1 & 0 & 0 \\ \Delta t & 1 & 0 \\ \frac{\Delta t^{2}}{2} & \Delta t & 1 \\ \end{matrix} \right] \sigma^{2}_{a} = \]

\[ = \left[ \begin{matrix} 0 & 0 & \frac{\Delta t^{2}}{2} \\ 0 & 0 & \Delta t \\ 0 & 0 & 1 \\ \end{matrix} \right] \left[ \begin{matrix} 1 & 0 & 0 \\ \Delta t & 1 & 0 \\ \frac{\Delta t^{2}}{2} & \Delta t & 1 \\ \end{matrix} \right] \sigma^{2}_{a} = \]

\[ = \left[ \begin{matrix} \frac{\Delta t^{4}}{4} & \frac{\Delta t^{3}}{2} & \frac{\Delta t^{2}}{2} \\ \frac{\Delta t^{3}}{2} & \Delta t^{2} & \Delta t \\ \frac{\Delta t^{2}}{2} & \Delta t & 1 \\ \end{matrix} \right] \sigma^{2}_{a} \]

入力係数行列を用いた変換

運動モデルに入力が含まれる場合、行列 \( \boldsymbol{Q} \) をより早く求めることができます。入力係数行列を用いて加速度の分散 \( \sigma^{2}_{a} \) を動的モデルに射影することができます。

\[ \boldsymbol{Q} = \boldsymbol{G}\sigma^{2}_{a}\boldsymbol{G^{T}} \]

ここで、\( \boldsymbol{G} \) は入力係数行列です。

運動モデルによって、入力係数行列 \( \boldsymbol{G} \) は次のように求められます。

\[ \boldsymbol{G} = \left[ \begin{matrix} \frac{\Delta t^{2}}{2} \\ \Delta t \\ \end{matrix} \right] \]

\[ \boldsymbol{Q} = \boldsymbol{G}\sigma^{2}_{a}\boldsymbol{G^{T}} = \sigma^{2}_{a}\boldsymbol{G}\boldsymbol{G^{T}} = \sigma^{2}_{a} \left[ \begin{matrix} \frac{\Delta t^{2}}{2} \\ \Delta t \\ \end{matrix} \right] \left[ \begin{matrix} \frac{\Delta t^{2}}{2} & \Delta t \\ \end{matrix} \right] = \sigma^{2}_{a} \left[ \begin{matrix} \frac{\Delta t^{4}}{4} & \frac{\Delta t^{3}}{2} \\ \frac{\Delta t^{3}}{2} & \Delta t^{2} \\ \end{matrix} \right] \]

上記の方法によって、離散的な行列 \( \boldsymbol{Q} \) を求めることができます。

連続雑音モデル

連続雑音モデルは、雑音が時間によって連続的に変換することを仮定しています。

連続モデルにおけるプロセス雑音の共分散行列 \( \boldsymbol{Q_{C}} \) を導出するために、離散モデルにおけるプロセス雑音の共分散行列 \( \boldsymbol{Q} \) を時間積分する必要があります。

\[ \boldsymbol{Q_{C}} = \int _{0}^{ \Delta t}\boldsymbol{Q}dt = \int _{0}^{ \Delta t} \sigma^{2}_{a} \left[ \begin{matrix} \frac{t^{4}}{4} & \frac{t^{3}}{2} \\ \frac{t^{3}}{2} & t^{2} \\ \end{matrix} \right] dt = \sigma^{2}_{a} \left[ \begin{matrix} \frac{\Delta t^{5}}{20} & \frac{\Delta t^{4}}{8} \\ \frac{\Delta t^{4}}{8} & \frac{\Delta t^{3}}{3} \\ \end{matrix} \right] \]

どちらのモデルを選択するべきでしょうか？

この質問に答える前に、プロセス雑音の分散について正しい値を使用する必要があります。確率統計の公式を使って計算してもよいですし、工学的な実践に基づいて妥当な値を選択してもよいでしょう（どちらが望ましいか、個人が選択する必要があります）。

レーダの世界では、\( \sigma^{2}_{a} \) はターゲットの特性やモデルの完成度に依存します。航空機のような機動性のあるターゲットに対しては \( \sigma^{2}_{a} \) はかなり大きくなります。ロケットのような非機動型のターゲットには、より小さな \( \sigma^{2}_{a} \) を使用することができます。また、モデルの完成度もプロセスノイズの分散を選択する際の要素になります。空気抵抗のような環境の影響を含むモデルであれば、プロセスノイズのランダム性の程度は小さくなり、その逆もまた然りです。

プロセスノイズの分散を適切に決定した後には、ノイズのモデル（離散型・連続型）を選択する必要があります。

この質問には、明確な答えはありません。もし、\( \Delta t \) が十分小さいのであれば、離散雑音モデルを選択できるでしょう。もし、\( \Delta t \) が大きいのであれば、連続雑音モデルを使う方がよいでしょう。あなたのカルマンフィルタに対して両方のモデルを試してみて、どちらがより良い性能を発揮できるか確認することをオススメします。