まとめ
5つの全てのカルマンフィルタの式を行列表記で導出しました。これらをこの1つのページにまとめます。
カルマンフィルタでは、次の図で表されるように、“ 予測 – 修正 ” ループが行われます。
1度初期化が行われると、カルマンフィルタは次時点の状態を予測します。また、予測の不確かさが求められます。
観測値が得られると、カルマンフィルタは前回の時点で予測されていた現時点での状態および不確かさの更新（または修正）を行います。同様に、カルマンフィルタは次の状態を予測し、これが続いていきます。
次の図は、カルマンフィルタの動作を図で表しています。
次の表に、全てのカルマンフィルタの式をまとめます。
| 式 | 名前 | 別名 | |
|---|---|---|---|
| 予測 | \( \boldsymbol{\hat{x}_{n+1,n}=F\hat{x}_{n,n}+Gu_{n}} \) | 状態方程式 |
Predictor Equation Transition Equation Prediction Equation Dynamic Model State Space Model |
| \( \boldsymbol{P_{n+1,n} = FP_{n,n}F^{T} + Q} \) | 共分散遷移式 | Predictor Covariance Equation | |
|
更新 （修正） |
\( \boldsymbol{\hat{x}_{n,n} = \hat{x}_{n,n-1} + K_{n} ( z_{n} - H \hat{x}_{n,n-1} )} \) | 状態更新式 | Filtering Equation |
| \( \boldsymbol{ P_{n,n} = \left( I - K_{n}H \right) P_{n,n-1} \left( I - K_{n}H \right)^{T} + K_{n}R_{n}K_{n}^{T} } \) | 共分散更新式 | Corrector Equation | |
| \( \boldsymbol{ K_{n} = P_{n,n-1}H^{T}\left( HP_{n,n-1}H^{T} + R_{n} \right)^{-1} } \) | カルマンゲイン | Weight Equation | |
| 補足 | \( \boldsymbol{z_{n} = Hx_{n}} \) | 観測方程式 | |
| \( \boldsymbol{R_{n}} = E\left( \boldsymbol{v_{n}v_{n}^{T}} \right) \) | 観測の不確かさ | Measurement Error | |
| \( \boldsymbol{Q_{n}} = E\left( \boldsymbol{w_{n}w_{n}^{T}} \right) \) | プロセス雑音の不確かさ | Process Noise Error | |
| \( \boldsymbol{P_{n,n}} = E\left( \boldsymbol{e_{n}e_{n}^{T}} \right) = E\left( \left( \boldsymbol{x_{n} - \hat{x}_{n,n}} \right) \left( \boldsymbol{x_{n} - \hat{x}_{n,n}} \right)^{T} \right) \) | 推定値の不確かさ | Estimation Error |
次の表に、記法と次元をまとめます（他の文献で見られる記法も含む）。
| 記号 | 名前 | 他の記法 | 次元 |
|---|---|---|---|
| \( \boldsymbol{x} \) | 状態ベクトル | \( n_{x} \times 1 \) | |
| \( \boldsymbol{z} \) | 観測ベクトル | \( \boldsymbol{y} \) | \( n_{z} \times 1 \) |
| \( \boldsymbol{F} \) | システム行列 | \( \boldsymbol{\Phi,A} \) | \( n_{x} \times n_{x} \) |
| \( \boldsymbol{u} \) | 入力変数 | \( n_{u} \times 1 \) | |
| \( \boldsymbol{G} \) | 入力係数行列 | \( \boldsymbol{B} \) | \( n_{x} \times n_{u} \) |
| \( \boldsymbol{P} \) | 推定の不確かさ | \( \boldsymbol{\Sigma} \) | \( n_{x} \times n_{x} \) |
| \( \boldsymbol{Q} \) | プロセス雑音の不確かさ | \( n_{x} \times n_{x} \) | |
| \( \boldsymbol{R} \) | 観測の不確かさ | \( n_{z} \times n_{z} \) | |
| \( \boldsymbol{w} \) | プロセス雑音ベクトル | \( \boldsymbol{y} \) | \( n_{x} \times 1 \) |
| \( \boldsymbol{v} \) | 観測雑音ベクトル | \( n_{z} \times 1 \) | |
| \( \boldsymbol{H} \) | 観測行列 | \( \boldsymbol{C} \) | \( n_{z} \times n_{x} \) |
| \( \boldsymbol{K} \) | カルマンゲイン | \( n_{x} \times n_{z} \) | |
| \( \boldsymbol{n} \) | 離散時間のインデックス | \( \boldsymbol{k} \) |
次元の記法
- \( n_{x} \)：状態ベクトルの状態量の数
- \( n_{z} \)：観測される状態量の数
- \( n_{u} \)：入力変数の要素の数