カルマンフィルタ（Kalman Filter）

予測の必要性

なぜ状態の推定と予測にアルゴリズムが必要なのかを理解するため、まず課題を定式化します。

例として、追尾レーダーを考えてみましょう。

航空機を追尾するレーダーを考えます。このとき航空機はシステムであり、推定すべき量は位置、すなわち システム状態です。

レーダーは細いビームを目標に向けて走査し、航空機の位置を測定します。これらの測定に基づいて、 システム状態（航空機の位置）を推定できます。

継続して追尾するには、レーダーは一定間隔で再び目標方向へビームを向ける必要があります。 つまり、次に照射する際の航空機の将来位置を予測できなければなりません。予測に失敗すると、 ビームが見当違いの方向を向き、追尾を失う恐れがあります。予測を行うには、航空機の運動に関する知識、 すなわち時間発展を記述する動力学モデル（dynamic model）が必要です。

例を簡単にするため、航空機がレーダーに向かうか離れるか、直線上を動く一次元の世界を考えます。

システム状態は、レーダーから航空機までの距離 \( r \) と定義します。レーダーは航空機に向けてパルスを送信し、 その反射が戻ってきます。送受信の時間差と、パルス（電磁波）が光速で伝搬することを用いれば、 距離 \( r \) を容易に求められます。さらに、ドップラー効果を用いた速度測定と同様に、航空機の速度 \( v \) も測れます。

時刻 \( t_{0} \) に、レーダーが距離と速度を高精度に測定したとします。距離は 10,000 m、 速度は 200 m/s と測定されました。これがシステム状態です。

\[ r_{t_{0}} = 10,000m \]

次に、目標再訪間隔を \( \Delta t \) として、時刻 \( t_{1}=t_{0}+\Delta t \) の状態を予測します。 航空機が等速で飛行すると仮定すれば、等速の動力学モデル（dynamic model）で将来位置を予測できます。

時間 \( \Delta t \) の間に移動する距離は次式で与えられます。

\[ \Delta r = v \cdot \Delta t \]

サンプリング間隔が 5 秒のとき、時刻 \( t_{1} \) の予測位置は次のとおりです。

\[ r_{t_{1}} = r_{t_{0}} + \Delta r = 10,000 + 200 \cdot 5 = 11,000m \]

これはごく基本的なアルゴリズムです。現在のシステム状態は測定から求め、将来状態は動力学モデルで予測します。

実世界では状況はもっと複雑です。まず、レーダー測定は完全ではなく、ノイズの影響を受けてばらつきます。 同時に 10 台のレーダーで同じ距離を測れば、互いに近いものの、少しずつ異なる 10 個の結果が得られるでしょう。 このばらつきは測定ノイズ（Measurement Noise）が原因です。

ここで新たな疑問が生じます。その推定はどれほど確かか。推定値だけでなく、信頼度も与えるアルゴリズムが必要です。

もう一つの問題は動力学モデルの正確さです。等速を仮定しても、風などの外乱により実際の運動はずれることがあります。 こうした予測不能な影響はプロセスノイズ（Process Noise）と呼ばれます。

測定に基づく推定の確かさを評価したいのと同様に、予測の信頼度も把握したいものです。

カルマンフィルタは、現在状態の推定と将来状態の予測を行い、さらにそれらの不確かさも与える 状態推定アルゴリズムです。加えて、推定の不確かさを最小化する最適アルゴリズムでもあります。 そのため、カルマンフィルタは幅広く用いられ、信頼されているのです。

カルマンフィルタの例

まずは単純な例から始めます。一次元レーダーが、航空機に向けてパルスを送信し、その反射波を受信して 距離と速度を測定します。送信から受信までの時間差は距離 \(r\) の情報を、反射波の周波数シフトは 速度 \(v\) の情報（ドップラー効果）を与えます。

この例では、システム状態は距離 \(r\) と速度 \(v\) の 2 つで表されます。両者を含む 状態ベクトル（State Vector） \(\boldsymbol{x}\) を次のように定義します。

\[ \boldsymbol{x}=\left[\begin{matrix}r\\v\\\end{matrix}\right] \]

本サイトでは、小文字の太字をベクトル、大文字の太字を行列で表します。

状態が複数の変数から成るため、ベクトルや行列などの線形代数の道具を用いてカルマンフィルタの数式を表します。 線形代数に不慣れな方は、オンラインチュートリアルの1次元カルマンフィルタをご覧ください。 高校数学レベルで式の導出を示し、4 つの完全な例題を掲載しています。

反復 0

フィルタの初期化

この例では、最初の測定値でカルマンフィルタを初期化します（初期化手法と性能への影響の詳細は、 書籍の第 21 章を参照）。時刻 \(t_0\) に距離 \(10{,}000\,m\) と速度 \(200\,m/s\) を観測しました。 測定は \(\boldsymbol{z}\) で表します。
測定値を観測ベクトル（Measurement Vector） \(\boldsymbol{z}\) に積み上げます。

\[ \boldsymbol{z}_0=\left[\begin{matrix}10{,}000\\200\\\end{matrix}\right] \]

下付きの \(0\) は時刻 \(t_0\) を示します。

測定値は真の状態そのものではありません。測定にはランダムノイズが含まれるため、各測定は確率変数です。

この測定はどの程度信頼できるでしょうか。各測定には測定不確かさ（Measurement Uncertainty）の二乗、 すなわち分散（Variance）が伴います。分散については基礎 I、 測定不確かさの詳細は1次元カルマンフィルタを参照してください。

レーダーでは、受信信号に対する雑音の比（信号対雑音比）が高いほど測定分散は小さくなり、測定への信頼度が高まります。

次の図は、ノイズ下での低信号と高信号を比較したものです。

距離測定の標準偏差が \(4\,m\)、速度測定の標準偏差が \(0.5\,m/s\) とします。分散は標準偏差の二乗なので、 測定共分散（Measurement Covariance） \(\boldsymbol{R}\) は次のとおりです。

\[ \boldsymbol{R}_0=\left[\begin{matrix}16&0\\0&0.25\\\end{matrix}\right] \]

\(\boldsymbol{R}\) は共分散行列（Covariance Matrix）で、対角要素に分散、非対角要素に共分散を持ちます。

\[ \boldsymbol{R}=\left[\begin{matrix}\sigma_r^2&\sigma_{rv}^2\\[0.5em]\sigma_{vr}^2&\sigma_v^2\\\end{matrix}\right] \]

ここでは距離と速度の測定誤差は無相関と仮定し、非対角要素は 0 とします。

分散（Variance）や標準偏差（Standard Deviation）の復習はオンラインチュートリアルの 基礎 I、共分散行列の復習は基礎 IIを参照してください。

初期化時に利用できる情報は 1 回の測定だけです。この例では、測定とシステム状態は同じ量（\(r\) と \(v\)）で表されるため、 測定値を初期の状態推定として用いることができます（初期化時のみ可能）。

\[ \boldsymbol{\hat{x}}_{0,0}=\boldsymbol{z}_0=\left[\begin{matrix}10{,}000\\200\\\end{matrix}\right] \]

下付き \(0,0\) の意味は次のとおりです。

• 1 つ目の添字はシステムの時刻（ここでは \(t_0\)）。
• 2 つ目の添字は推定を行った時刻（同じく \(t_0\)）。

つまり、\(t_0\) の状態を、\(t_0\) の情報で推定しています。

予測

次の状態を予測します。再訪時間を 5 秒（\(\Delta t=5s\)）とすると、\(t_1=5s\) です。

将来の状態を見積もるには、時間発展の記述が必要です。ここでは等速の動力学モデル（運動モデル）を用います。

\[ v_{1} = v_{0} = v \] \[ r_{1} = r_{0} + v_{0}\Delta t \]

（加速度を含むモデルの例は、書籍第 9 章を参照）

この動力学を行列で表します。

\[ {\hat{\boldsymbol{x}}}_{1,0}=\boldsymbol{F}{\hat{\boldsymbol{x}}}_{0,0} \]

下付き \(1,0\) の意味は次のとおりです。

• 1 つ目の添字はシステム時刻（\(t_1\)）。
• 2 つ目の添字は推定を行った時刻（\(t_0\)）。

したがって \( \hat{\boldsymbol{x}}_{1,0} \) は、\(t_0\) の情報で計算した \(t_1\) の状態の予測値（Prediction）です。

行列 \(\boldsymbol{F}\) はシステム行列（State Transition Matrix）で、状態の時間発展を表します。

\[ {\hat{\boldsymbol{x}}}_{1,0}=\left[\begin{matrix}{\hat{r}}_{1,0}\\{\hat{v}}_{1,0}\\\end{matrix}\right]=\left[\begin{matrix}1&\Delta t\\0&1\\\end{matrix}\right]\left[\begin{matrix}{\hat{r}}_{0,0}\\{\hat{v}}_{0,0}\\\end{matrix}\right]=\left[\begin{matrix}1&5\\0&1\\\end{matrix}\right]\left[\begin{matrix}10,000\\200\\\end{matrix}\right]=\left[\begin{matrix}11,000\\200\\\end{matrix}\right] \]

任意の線形システムの動力学モデル化は、書籍付録 C に解説があります。

次式

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

は状態方程式（State Extrapolation Equation）です。現在の推定値と運動モデルから、次の時刻の状態を計算します。

入力を含む完全な形は次式です。

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

ここで、

• \(\boldsymbol{u}_{n}\)：入力（制御）ベクトル
• \(\boldsymbol{G}\)：入力係数行列（Control Matrix）

入力ベクトルは、機体搭載加速度計の読取りなど、カルマンフィルタに与える追加情報を表します。

ここでは入力はないと仮定し、\(\boldsymbol{u}_n=0\) とします。

入力項を含む例は、オンラインチュートリアルの状態方程式や、 書籍の完全解付き「例題 10」を参照してください。

すべての測定と推定には不確かさの情報が伴います。次状態の予測後には、その予測の精度も評価すべきです。

現在の状態推定の二乗不確かさは、共分散行列で表されます。

\[ \boldsymbol{P}_{0,0}=\left[\begin{matrix}16&0\\0&0.25\\\end{matrix}\right] \]

ただし、予測共分散は次式ではありません。

\[ \textcolor{red}{\xcancel{\textcolor{black}{ \boldsymbol{P}_{1,0}=\boldsymbol{F}\boldsymbol{P}_{0,0} }}} \]

\(\boldsymbol{P}\) は共分散行列であり、分散・共分散は二乗を含むためです。

プロセスノイズを含まない共分散遷移式は次のとおりです。

\[ \boldsymbol{P}_{n+1,n}=\boldsymbol{F}\boldsymbol{P}_{n,n}\boldsymbol{F}^T \]

導出はオンラインチュートリアルの共分散遷移式で解説しています。

本例では次のようになります。

$$ \boldsymbol{P}_{1,0}=\boldsymbol{F}\boldsymbol{P}_{0,0}\boldsymbol{F}^T=\left[\begin{matrix}1&5\\0&1\\\end{matrix}\right]\left[\begin{matrix}16&0\\0&0.25\\\end{matrix}\right]\left[\begin{matrix}1&0\\5&1\\\end{matrix}\right]=\left[\begin{matrix}1&5\\0&1\\\end{matrix}\right]\left[\begin{matrix}16&0\\1.25&0.25\\\end{matrix}\right]=\left[\begin{matrix}\colorbox{yellow}{$22.25$}&1.25\\1.25&\colorbox{yellow}{$0.25$}\\\end{matrix}\right] $$

共分散行列の主対角に注目します。

速度分散 \(\sigma_v^2\) は \(0.25\,m^2/s^2\) のままです。等速モデルでは速度に不確かさが増えないためです。

一方、距離分散 \(\sigma_r^2\) は \(16\,m^2\) から \(22.25\,m^2\) に増加します。速度の不確かさが時間とともに 距離の不確かさを増加させるためです。

先に述べたように、等速の仮定は完全ではありません。実際には風などの未知の外乱で速度が変動し、 予測の不確かさは単純モデルの見積りより大きくなります。

こうした予測不能な影響をプロセスノイズ（Process Noise）と呼び、\(\boldsymbol{Q}\) で表します。 これを考慮するため、予測共分散に \(\boldsymbol{Q}\) を加えます。

\[ \boldsymbol{P}_{n+1,n}=\boldsymbol{F}\boldsymbol{P}_{n,n}\boldsymbol{F}^T + \boldsymbol{Q}\]

プロセスノイズが性能に与える影響の直感は、オンラインチュートリアルの例題 6で確認できます。

乱加速度の標準偏差を \(\sigma_a=0.2\,m/s^2\) とします。これは環境の不確定要因による加速度の不確かさを表します。

よって分散は \(\sigma_a^2=0.04\,m^2/s^4\) です。

このとき、プロセスノイズ行列（Process Noise Matrix）は次式です。

$$ \boldsymbol{Q} = \left[\begin{matrix} \frac{\Delta t^4}{4} & \frac{\Delta t^3}{2} \\[0.5em] \frac{\Delta t^3}{2} & \Delta t^2 \end{matrix}\right] \sigma_a^2 $$

\(\Delta t=5\,s\)、\(\sigma_a^2=0.04\,m^2/s^4\) のとき、

$$ \boldsymbol{Q}=\left[\begin{matrix}\frac{625}{4}&\frac{125}{2}\\[0.5em] \frac{125}{2}&25\\\end{matrix}\right]0.04=\left[\begin{matrix}6.25&2.5\\2.5&1\\\end{matrix}\right] $$

導出は書籍の 8.2.2 節を参照してください。

プロセスノイズを加えると、予測の二乗不確かさは次のとおりです。

$$ \boldsymbol{P}_{1,0}=\boldsymbol{F}\boldsymbol{P}_{0,0}\boldsymbol{F}^T+\boldsymbol{Q}\ =\left[\begin{matrix}22.25&1.25\\1.25&0.25\\\end{matrix}\right]+\left[\begin{matrix}6.25&2.5\\2.5&1\\\end{matrix}\right]\ =\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right] $$

反復 0 のまとめ

• 初期化
  最初の測定を初期の状態推定 \( {\hat{\boldsymbol{x}}}_{0,0} \) とし、測定共分散を初期の状態共分散 \(\boldsymbol{P}_{0,0}\) としてカルマンフィルタを初期化しました（初期化段階のみ実施可能）。
• 予測
  レーダーが再訪する次の時刻の状態と不確かさを予測しました。カルマンフィルタの予測式は次のとおりです。
  
  
  状態方程式（State Extrapolation Equation）
  \[ {\hat{\boldsymbol{x}}}_{n+1,n}=\boldsymbol{F}{\hat{\boldsymbol{x}}}_{n,n} + \boldsymbol{G}\boldsymbol{u}_n \]
  共分散遷移式（Covariance Extrapolation）
  \[ \boldsymbol{P}_{n+1,n}=\boldsymbol{F}\boldsymbol{P}_{n,n}\boldsymbol{F}^T + \boldsymbol{Q}\]
  ここで、
  
  • \(\hat{\boldsymbol{x}}_{n,n}\)：時刻 \(n\) の状態推定ベクトル
  • \(\hat{\boldsymbol{x}}_{n+1,n}\)：時刻 \(n\) の情報で計算した、時刻 \(n+1\) の予測状態ベクトル
  • \(\boldsymbol{u}_n\)：入力（制御）ベクトル
  • \(\boldsymbol{F}\)：システム行列（状態遷移行列）
  • \(\boldsymbol{G}\)：入力係数行列
  • \(\boldsymbol{P}_{n,n}\)：現在状態の共分散
  • \(\boldsymbol{P}_{n+1,n}\)：予測状態の共分散
  • \(\boldsymbol{Q}\)：プロセスノイズ行列

反復 1

フィルタの更新

時刻 \(t_1\) の 2 回目の測定を仮定します。

\[ \boldsymbol{z}_1=\left[\begin{matrix}11{,}020\\202\\\end{matrix}\right] \]

今回の測定ではノイズスパイクが強く、信号対雑音比が初回より大幅に低下しました。その結果、2 回目の測定の 不確かさは大きくなります。

距離測定の標準偏差を \(6\,m\)、速度測定の標準偏差を \(1.5\,m/s\) とすると、測定共分散は次のとおりです。

\[ \boldsymbol{R}_1=\left[\begin{matrix}\colorbox{yellow}{$36$}&0\\0&\colorbox{yellow}{$2.25$}\\\end{matrix}\right] \]

現在のシステム状態 \(\hat{\boldsymbol{x}}_{1,1}\) を推定します。時刻 \(t_1\) には 2 つの情報があります。

• 予測状態 \(\hat{\boldsymbol{x}}_{1,0}\)（前段で計算）
• 新しい測定 \(\boldsymbol{z}_1\)

どちらをより信頼すべきでしょうか。

直感的には、最新の情報である測定をそのまま採用（\(\hat{\boldsymbol{x}}_{1,1}=\boldsymbol{z}_1\)）したくなります。

しかし、測定にはノイズが多く含まれます。予測共分散 \(\boldsymbol{P}_{1,0}\) と測定共分散 \(\boldsymbol{R}_1\) の 主対角を比べると、予測の不確かさのほうが小さいことが分かります。

\[ \boldsymbol{P}_{1,0}=\left[\begin{matrix}\colorbox{yellow}{$28.5$}&3.75\\3.75&\colorbox{yellow}{$1.25$}\\\end{matrix}\right] \]

それなら新しい測定を無視して予測を維持（\(\hat{\boldsymbol{x}}_{1,1}=\hat{\boldsymbol{x}}_{1,0}\)）すべきでしょうか。

その場合は、測定がもたらす新情報を失ってしまいます。

カルマンフィルタの要点はどちらか一方にしないことです。予測と測定を組み合わせ、 不確かさの小さいほうに重みを大きく与えます。

解は、測定と予測の加重平均です。

\[ \hat{x}_{1,1}=K_1 z_1\ +\ \left({1-\ K}_1\right){\hat{x}}_{1,0}, \quad 0\leq K_1 \leq 1 \]

重み \(K_1\) はカルマンゲイン（Kalman Gain）です。推定の不確かさが最小になるように、 測定と予測の寄与を決めます。これにより、モデルの仮定が成り立つ範囲でカルマンフィルタは最適となります。

カルマンゲインの式は後で示すとして、まず状態更新式（State Update Equation）を示します。行列表現は次のとおりです。

\[ \hat{\boldsymbol{x}}_{1,1}=\boldsymbol{K}_1\boldsymbol{z}_1 + (\boldsymbol{I} - \boldsymbol{K}_1)\hat{\boldsymbol{x}}_{1,0} \]

ここで \(\boldsymbol{I}\) は単位行列（Identity Matrix）です。

式を変形すると、

\[ \hat{\boldsymbol{x}}_{1,1}=\boldsymbol{K}_1\boldsymbol{z}_1 + \hat{\boldsymbol{x}}_{1,0} - \boldsymbol{K}_1\hat{\boldsymbol{x}}_{1,0}=\hat{\boldsymbol{x}}_{1,0}+\boldsymbol{K}_1(\boldsymbol{z}_1 - \hat{\boldsymbol{x}}_{1,0}) \]

これは、更新後の状態が予測 \(\hat{\boldsymbol{x}}_{1,0}\) に、補正項 \(\boldsymbol{K}_1(\boldsymbol{z}_1 - \hat{\boldsymbol{x}}_{1,0})\) を加えたものであることを示します。

補正量は測定と予測の差 \(\boldsymbol{z}_1 - \hat{\boldsymbol{x}}_{1,0}\) に比例し、これはイノベーション（innovation） または残差と呼ばれます。

この例では、状態と測定はいずれも距離と速度を表すベクトルなので、\(\boldsymbol{z}_1\) から \(\hat{\boldsymbol{x}}_{1,0}\) を直接引けます。

しかし常にそうとは限りません。一般には、測定と状態は異なる物理領域に属することがあります。 例えば、デジタル温度計は電気信号を測定しますが、状態は温度です。

そのため、予測状態を観測行列（Observation Matrix） \(\boldsymbol{H}\) により観測空間へ写像します。

\[ \boldsymbol{H} \hat{\boldsymbol{x}}_{1,0} \]

\(\boldsymbol{H}\) は観測（測定）行列で、状態を実際に観測される量に対応づけます。

本例では \(\boldsymbol{H}\) は単位行列です。

\[ \boldsymbol{H}=\left[\begin{matrix}1&0\\0&1\\\end{matrix}\right]=\boldsymbol{I} \]

観測行列の詳細は、オンラインチュートリアルの観測方程式や、 書籍の例題 9・10 を参照してください。

状態更新式は次のように書けます。

\[ \hat{\boldsymbol{x}}_{1,1}=\hat{\boldsymbol{x}}_{1,0}+\boldsymbol{K}_1(\boldsymbol{z}_1 - \boldsymbol{H}\hat{\boldsymbol{x}}_{1,0}) \]

イノベーション \(\boldsymbol{z}_1 - \boldsymbol{H}\hat{\boldsymbol{x}}_{1,0}\) は新情報を表します。

カルマンゲインは、新情報で予測をどの程度補正するか（修正の強さ）を定めます。

1次元の場合

1 次元の場合、カルマンゲインは次式で与えられます。

\[ K_n=\frac{p_{n,\ n-1}}{p_{n,\ n-1}+r_n} \]

ここで、

• \(p_{n,\ n-1}\)：予測状態の分散
• \(r_n\)：測定の分散

カルマンゲインは更新後の推定分散 \(p_{n,n}\) を最小化するように選ばれます。これがカルマンフィルタが最適である理由です。

直感と導出の詳細は、オンラインチュートリアルの1次元カルマンフィルタを参照してください。

多次元の場合

多次元のカルマンフィルタでは、カルマンゲインは行列となり、次式で与えられます。

\[ \boldsymbol{K}_n=\boldsymbol{P}_{n,n-1}\boldsymbol{H}^T\left(\boldsymbol{H}\boldsymbol{P}_{n,n-1}\boldsymbol{H}^T+\boldsymbol{R}_n\right)^{-1} \]

導出はオンラインチュートリアルのカルマンゲインを参照してください。

\(t_1\) のカルマンゲインを計算します。

\[ \boldsymbol{K}_1=\boldsymbol{P}_{1,0}\boldsymbol{H}^T\left(\boldsymbol{H}\boldsymbol{P}_{1,0}\boldsymbol{H}^T+\boldsymbol{R}_1\right)^{-1} \]

本例では \(\boldsymbol{H}=\boldsymbol{I}\)、\(\boldsymbol{H}^T=\boldsymbol{I}\) です。

行列を代入します。

\[ \boldsymbol{P}_{1,0}=\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right], \quad \boldsymbol{R}_1=\left[\begin{matrix}36&0\\0&2.25\\\end{matrix}\right] \]

\[ \boldsymbol{K}_1=\boldsymbol{P}_{1,0}\boldsymbol{H}^T\left(\boldsymbol{H}\boldsymbol{P}_{1,0}\boldsymbol{H}^T+\boldsymbol{R}_1\right)^{-1}=\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]\left[\begin{matrix}1&0\\0&1\\\end{matrix}\right]\left(\left[\begin{matrix}1&0\\0&1\\\end{matrix}\right]\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]\left[\begin{matrix}1&0\\0&1\\\end{matrix}\right]+\left[\begin{matrix}36&0\\0&2.25\\\end{matrix}\right]\right)^{-1} \]

\[ =\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]\left(\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]+\left[\begin{matrix}36&0\\0&2.25\\\end{matrix}\right]\right)^{-1} =\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]\left(\left[\begin{matrix}64.5&3.75\\3.75&3.5\\\end{matrix}\right]\right)^{-1} \]

\[ =\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]\left[\begin{matrix}0.0165&-0.0177\\-0.0177&0.3047\\\end{matrix}\right]=\left[\begin{matrix}0.4048&0.6377\\0.0399&0.3144\\\end{matrix}\right] \]

\[ \boldsymbol{K}_1=\left[\begin{matrix}0.4048&0.6377\\0.0399&0.3144\\\end{matrix}\right] \]

更新後の状態推定は次のとおりです。

\[ \hat{\boldsymbol{x}}_{1,1}=\hat{\boldsymbol{x}}_{1,0}+\boldsymbol{K}_1(\boldsymbol{z}_1 - \boldsymbol{H}\hat{\boldsymbol{x}}_{1,0}) \]

本例では \(\boldsymbol{H}=\boldsymbol{I}\) なので、イノベーションは次のように簡単になります。

\[ \boldsymbol{z}_1 - \boldsymbol{I}\hat{\boldsymbol{x}}_{1,0}=\boldsymbol{z}_1 - \hat{\boldsymbol{x}}_{1,0}=\left[\begin{matrix}11{,}020\\202\\\end{matrix}\right] - \left[\begin{matrix}11{,}000\\200\\\end{matrix}\right]=\left[\begin{matrix}20\\2\\\end{matrix}\right] \]

補正を適用します。

\[ \boldsymbol{K}_1\left[\begin{matrix}20\\2\\\end{matrix}\right]=\left[\begin{matrix}0.4048&0.6377\\0.0399&0.3144\\\end{matrix}\right]\left[\begin{matrix}20\\2\\\end{matrix}\right]=\left[\begin{matrix}9.37\\1.43\\\end{matrix}\right] \]

最終的に、

\[ \hat{\boldsymbol{x}}_{1,1}=\left[\begin{matrix}11{,}000\\200\\\end{matrix}\right]+\left[\begin{matrix}9.37\\1.43\\\end{matrix}\right]=\left[\begin{matrix}11{,}009.37\\201.43\\\end{matrix}\right] \]

現在の状態を推定したら、その不確かさも定量化します。

1次元の場合

1 次元の場合の共分散更新式（Covariance Update Equation）は次式です。

\[ p_{n,n}=(1-K_n)p_{n,\ n-1} \]

導出はオンラインチュートリアルの1次元カルマンフィルタを参照してください。

多次元の場合

Joseph 形式

多次元カルマンフィルタでは、数値的に安定なジョセフ形式（Joseph form）で共分散更新式を書くのが一般的です。

\[ \boldsymbol{P}_{n,n}=(\boldsymbol{I} - \boldsymbol{K}_n\boldsymbol{H})\boldsymbol{P}_{n,n-1}(\boldsymbol{I} - \boldsymbol{K}_n\boldsymbol{H})^T + \boldsymbol{K}_n\boldsymbol{R}_n\boldsymbol{K}_n^T \]

ここで、

• \(\boldsymbol{P}_{n,n}\)：更新（事後）状態推定の共分散
• \(\boldsymbol{P}_{n,n-1}\)：予測（事前）状態推定の共分散
• \(\boldsymbol{K}_n\)：カルマンゲイン
• \(\boldsymbol{H}\)：観測（測定）行列
• \(\boldsymbol{R}_n\)：測定ノイズの共分散行列
• \(\boldsymbol{I}\)：単位行列

導出はオンラインチュートリアルの共分散更新式を参照してください。

簡略形

文献では、簡略形もよく用いられます。

\[ \boldsymbol{P}_{n,n}=(\boldsymbol{I} - \boldsymbol{K}_n\boldsymbol{H})\boldsymbol{P}_{n,n-1} \]

導出は共分散更新式の簡略化を参照してください。

厳密計算では同じ結果になりますが、数値計算ではジョセフ形式のほうが安定です。

本例では簡略形を用います。

\[ \boldsymbol{P}_{1,1}=(\boldsymbol{I} - \boldsymbol{K}_1\boldsymbol{H})\boldsymbol{P}_{1,0} \]

本例では \(\boldsymbol{H}=\boldsymbol{I}\) なので、

\[ \boldsymbol{P}_{1,1}=(\boldsymbol{I} - \boldsymbol{K}_1)\boldsymbol{P}_{1,0} \]

行列を代入すると、

\[ \boldsymbol{P}_{1,1}=\left(\left[\begin{matrix}1&0\\0&1\\\end{matrix}\right] - \left[\begin{matrix}0.4048&0.6377\\0.0399&0.3144\\\end{matrix}\right]\right)\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right] \]

\[ =\left[\begin{matrix}0.5952&-0.6377\\-0.0399&0.6856\\\end{matrix}\right]\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]=\left[\begin{matrix}14.57&1.43\\1.43&0.71\\\end{matrix}\right] \]

結果の考察

更新後の推定不確かさは、予測不確かさと測定不確かさのいずれよりも小さくなります。

\[ \boldsymbol{P}_{1,1}=\left[\begin{matrix}\colorbox{yellow}{$14.57$}&1.43\\1.43&\colorbox{yellow}{$0.71$}\\\end{matrix}\right]\ \ \ \ \ \ \boldsymbol{P}_{1,0}=\ \left[\begin{matrix}\colorbox{yellow}{$28.5$}&3.75\\3.75&\colorbox{yellow}{$1.25$}\\\end{matrix}\right]\ \ \ \ \ \boldsymbol{R}_\mathbf{1}=\left[\begin{matrix}\colorbox{yellow}{$36$}&0\\0&\colorbox{yellow}{$2.25$}\\\end{matrix}\right] \]

測定と予測をカルマンゲインで重み付けして組み合わせることで、不確かさの小さい推定が得られます。

不確かさが大きい情報であっても、新しい情報を加えれば推定の不確かさは必ず減少します。理論的な証明は 書籍のセンサフュージョンの章と付録 G・H を参照してください。理論的には、新しい測定は 無視すべきではありません。

ただし実務では、測定を棄却すべき場合もあります。信頼性の低い測定への対処は、 書籍の外れ値処理の章を参照してください。

予測

反復 1（\(t_1\) → \(t_2\)）の予測は、更新後の \(\hat{\boldsymbol{x}}_{1,1}\)、\(\boldsymbol{P}_{1,1}\) から開始する点を 除けば、反復 0（\(t_0\) → \(t_1\)）と同じです。

状態の予測

\[ \hat{\boldsymbol{x}}_{2,1}=\boldsymbol{F}\hat{\boldsymbol{x}}_{1,1} \]

\[ \hat{\boldsymbol{x}}_{2,1}=\left[\begin{matrix}1&5\\0&1\\\end{matrix}\right]\left[\begin{matrix}11,009.37\\201.43\\\end{matrix}\right]=\left[\begin{matrix}12,016.5\\201.43\\\end{matrix}\right] \]

共分散の予測

\[ \boldsymbol{P}_{2,1}=\boldsymbol{F}\boldsymbol{P}_{1,1}\boldsymbol{F}^\top + \boldsymbol{Q} \]

\[ \boldsymbol{P}_{2,1}=\ \left[\begin{matrix}1&5\\0&1\\\end{matrix}\right]\left[\begin{matrix}14.57&1.43\\1.43&0.71\\\end{matrix}\right]\left[\begin{matrix}1&0\\5&1\\\end{matrix}\right]+\left[\begin{matrix}6.25&2.5\\2.5&1\\\end{matrix}\right]=\left[\begin{matrix}52.86&7.47\\7.47&1.71\\\end{matrix}\right] \]

予測ではいずれの分散も再び増加します。新しい測定が得られない時間の経過により不確かさが自然に増えるためです。 特に速度の不確かさは距離の不確かさを増加させ、距離分散が速度分散より速く増える要因となります。

反復 1 のまとめ

• 更新
  
  • 現在の状態 \(\hat{\boldsymbol{x}}_{1,1}\) を、予測 \(\hat{\boldsymbol{x}}_{1,0}\) と測定 \(\boldsymbol{z}_1\) の 加重和として推定します。
    重みはカルマンゲイン \(K_1\) で、予測共分散 \(\boldsymbol{P}_{1,0}\) と測定共分散 \(\boldsymbol{R}_1\) から 計算され、更新後共分散 \(\boldsymbol{P}_{1,1}\) を最小化します。
  • カルマンフィルタの更新式は次のとおりです。
    
    
    状態更新式（State Update Equation）
    \[ \hat{\boldsymbol{x}}_{n,n}=\hat{\boldsymbol{x}}_{n,n-1}+\boldsymbol{K}_n\left(\boldsymbol{z}_n\ -\ \boldsymbol{H}\hat{\boldsymbol{x}}_{n,n-1}\right) \]
    共分散更新式（Joseph 形式）
    \[ \boldsymbol{P}_{n,n}=\left(\boldsymbol{I}-\boldsymbol{K}_n\boldsymbol{H}\right)\boldsymbol{P}_{n,n-1}\left(\boldsymbol{I}-\boldsymbol{K}_n\boldsymbol{H}\right)^T+\boldsymbol{K}_n\boldsymbol{R}_n\boldsymbol{K}_n^T \]
    または簡略形
    \[\boldsymbol{P}_{n,n}=\left(\boldsymbol{I}-\boldsymbol{K}_n\boldsymbol{H}\right)\boldsymbol{P}_{n,n-1}\]
    カルマンゲイン（Kalman Gain）の式
    \[ \boldsymbol{K}_n=\ \boldsymbol{P}_{n,n-1}\boldsymbol{H}^T\left(\boldsymbol{H}\boldsymbol{P}_{n,n-1}\boldsymbol{H}^T+\boldsymbol{R}_n\right)^{-1}\]
  ここで、
  
  • \( \hat{\boldsymbol{x}}_{n,n} \)：時刻 \(n\) の更新後状態
  • \( \hat{\boldsymbol{x}}_{n,n-1} \)：時刻 \(n-1\) の情報で計算した時刻 \(n\) の予測状態
  • \( \boldsymbol{z}_n \)：観測ベクトル
  • \( \boldsymbol{P}_{n,n} \)：更新後状態の共分散
  • \( \boldsymbol{P}_{n,n-1} \)：予測状態の共分散
  • \( \boldsymbol{K}_n \)：カルマンゲイン
  • \( \boldsymbol{H} \)：観測（測定）行列
  • \( \boldsymbol{R}_n \)：測定ノイズの共分散行列
  • \( \boldsymbol{I} \)：単位行列
• 予測
  反復 1 の予測は反復 0 と同様です。
  状態遷移モデルを用いて、現在の推定とその共分散を次の時刻まで伝播します。

例のまとめ