4 カルマンフィルタ

Wienerフィルタ

4.1 システムモデル

4.1.1 基本モデル

状態方程式と観測方程式。

$$\bm{x}_{k+1} = \bm{F}_k x_k + \bm{G}_k \bm{w}_k$$ (11)

$$\bm{y}_k = \bm{H}_k + \bm{v}_k, \quad k \geq 0$$ (12)

図 5: 状態空間モデルのブロック線図。

4.1.2 確率的仮定

図 6: 2次元のGauss分布の例

4.1.3 モデルの性質

4.1.4 統計量の伝搬

4.2 最適推定

4.2.1 最大事後推定値

4.2.2 最小分散推定値

4.3 カルマンフィルタリング問題

観測値 $\bm{y}_0, \cdots, \bm{y}_k$が与えられた時の$\bm{x}_k$の最小分散推定値 $\hat{\bm{x}}_{k\vert k} = E\{ \bm{x}_k \vert \bm{y}_0, \cdots, \bm{y}_k \}$または $\hat{\bm{x}}_{k\vert k-1} = E\{ \bm{x}_k \vert \bm{y}_0, \cdots, \bm{y}_{k-1} \}$を求める問題をカルマンフィルタリング問題 カルマンフィルタリング問題（Kalman filtering problem）と言い、 その解を与えるアルゴリズムをカルマンフィルタ（Kalman filter）と呼ぶ。

4.3.1 カルマンフィルタ

フィルタ方程式

$$\hat{\bm{x}}_{k\vert k} = \hat{\bm{x}}_{k\vert k-1} + \bm{K}_{k} (\bm{y}_{k} - \bm{H}_{k} \hat{\bm{x}}_{k\vert k-1})$$ (13)

$$\hat{\bm{x}}_{k+1\vert k} = \bm{F}_{k} \hat{\bm{x}}_{k\vert k}$$ (14)

カルマンゲイン

$$\bm{K}_{k} = \hat{\bm{\Sigma}}_{k\vert k-1} \bm{H}_{k}^T (\bm{H}_{k} \hat{\bm{\Sigma}}_{k\vert k-1} \bm{H}_{k}^T + \bm{\Sigma}_{\bm{v}_k})^{-1}$$ (15)

誤差の共分散行列方程式

$$\hat{\bm{\Sigma}}_{k\vert k} = \hat{\bm{\Sigma}}_{k\vert k-1} - \bm{K}_{k} \bm{H}_{k} \hat{\bm{\Sigma}}_{k\vert k-1}$$ (16)

$$\hat{\bm{\Sigma}}_{k+1\vert k} = \bm{F}_{k} \hat{\bm{\Sigma}}_{k\vert k} \bm{F}_{k}^T + \bm{G}_{k} \bm{\Sigma}_{\bm{w}_k} \bm{G}_{k}^T$$ (17)

初期値

$$\hat{\bm{x}}_{0\vert-1} = \bar{\bm{x}}_0 , \quad \hat{\bm{\Sigma}}_{0\vert-1} = \bm{\Sigma}_{\bm{x}_0}$$ (18)

図 7: Kalmanフィルタのブロック線図。

4.3.2 1段予測型カルマンフィルタ

カルマンフィルタの1段予測値 $\hat{\bm{x}}_{k+1\vert k}$に関して整理しなおすと次のようにまとめられる。

フィルタ方程式

$$\hat{\bm{x}}_{k+1\vert k} = (\bm{F}_{k} - \bm{L}_{k} \bm{H}_{k} ) \hat{\bm{x}}_{k\vert k-1} + \bm{L}_{k} \bm{y}_{k}$$ (19)

カルマンゲイン

$$\bm{L}_{k} = \bm{F}_{k} \hat{\bm{\Sigma}}_{k\vert k-1} \bm{H}_{k}... ..._{k} \hat{\bm{\Sigma}}_{k\vert k-1} \bm{H}_{k}^T + \bm{\Sigma}_{\bm{v}_k})^{-1}$$ (20)

誤差の共分散行列方程式

$$\hat{\bm{\Sigma}}_{k+1\vert k} = \bm{F}_{k} \hat{\bm{\Sigma}}_{k\vert k-1} \bm{F}_{k}^T$$

$$- \bm{F}_{k} \hat{\bm{\Sigma}}_{k\vert k-1} \bm{H}_{k}^T (\bm{H}_... ...\Sigma}_{\bm{v}_k})^{-1} \bm{H}_{k} \hat{\bm{\Sigma}}_{k\vert k-1} \bm{F}_{k}^T$$

$$+ \bm{G}_{k} \hat{\bm{\Sigma}}_{\bm{w}_k} \bm{G}_{k}^T$$ (21)

初期値

$$\hat{\bm{x}}_{0\vert-1} = \bar{\bm{x}}_0 , \quad \hat{\bm{\Sigma}}_{0\vert-1} = \bm{\Sigma}_{\bm{x}_0}$$ (22)

1段予測型KalmanフィルタとKalmanフィルタは等価であり、 $\bm{L}_{k} = \bm{F}_{k}\bm{K}_{k}$の関係がある。

図 8: 1段予測型Kalmanフィルタのブロック線図。

4.3.3 線形最小分散フィルタ

源信号 $\bm{x}_{k}$と観測信号 $\{\bm{y}_{k}\}$が結合Gauss性であるとき、Kalmanフィルタは非線形フィルタも含めて、あらゆるフィルタの中で最も最適なフィルタであった。しかし、現実の問題ではGauss性の仮定は必ずしも保証されない。そこで、非Gauss性の場合への拡張を行う。

フィルタ方程式

$$\hat{\bm{x}}_{k\vert k} = \hat{\bm{x}}_{k\vert k-1} + \bm{K}_{k} (\bm{y}_{k} - \bm{H}_{k} \hat{\bm{x}}_{k\vert k-1})$$ (23)

$$\hat{\bm{x}}_{k+1\vert k} = \bm{F}_{k} \hat{\bm{x}}_{k\vert k}$$ (24)

カルマンゲイン

$$\bm{K}_{k} = \hat{\bm{\Sigma}}_{k\vert k-1} \bm{H}_{k}^T (\bm{H}_{k} \hat{\bm{\Sigma}}_{k\vert k-1} \bm{H}_{k}^T + \bm{\Sigma}_{\bm{v}_k})^{-1}$$ (25)

誤差の共分散行列方程式

$$\hat{\bm{\Sigma}}_{k\vert k} = \hat{\bm{\Sigma}}_{k\vert k-1} - \bm{K}_{k} \bm{H}_{k} \hat{\bm{\Sigma}}_{k\vert k-1}$$ (26)

$$\hat{\bm{\Sigma}}_{k+1\vert k} = \bm{F}_{k} \hat{\bm{\Sigma}}_{k\vert k} \bm{F}_{k}^T + \bm{G}_{k} \bm{\Sigma}_{\bm{w}_k} \bm{G}_{k}^T$$ (27)

初期値

$$\hat{\bm{x}}_{0\vert-1} = \bar{\bm{x}}_0 , \quad \hat{\bm{\Sigma}}_{0\vert-1} = \bm{\Sigma}_{\bm{x}_0}$$ (28)

Kalmanフィルタを導入した際にはGauss性を仮定してフィルタを構築したが、Gauss性の無い場合にもそのまま成り立つ事が分かった。 ただし、Gauss性でない場合のKalmanフィルタは、あらゆるフィルタの中で最も最適なフィルタであるとは限らない。

4.3.4 複素カルマンフィルタ

データ通信や計測分野などへの応用では、システムの状態空間モデルは複素数で定義した方が都合がよい。そこで、Kanlamフィルタを複素数へ拡張する。

ここで、入力信号にはGauss性に代わって複素Gauss性

$$p_x(\bm{x}) = \frac{1}{\pi^N \vert\bm{\Sigma}_{\bm{x}}\vert} \exp... ...m{x} - \bar{\bm{x}})^\dag\bm{\Sigma}_{\bm{x}}^{-1} (\bm{x} - \bar{\bm{x}}) \} }$$ (29)

を仮定する。

フィルタ方程式

$$\hat{\bm{x}}_{k\vert k} = \hat{\bm{x}}_{k\vert k-1} + \bm{K}_{k} (\bm{y}_{k} - \bm{H}_{k} \hat{\bm{x}}_{k\vert k-1})$$ (30)

$$\hat{\bm{x}}_{k+1\vert k} = \bm{F}_{k} \hat{\bm{x}}_{k\vert k}$$ (31)

カルマンゲイン

$$\bm{K}_{k} = \hat{\bm{\Sigma}}_{k\vert k-1} \bm{H}_{k}^\dag (\bm{... ...} \hat{\bm{\Sigma}}_{k\vert k-1} \bm{H}_{k}^\dag + \bm{\Sigma}_{\bm{v}_k})^{-1}$$ (32)

誤差の共分散行列方程式

$$\hat{\bm{\Sigma}}_{k\vert k} = \hat{\bm{\Sigma}}_{k\vert k-1} - \bm{K}_{k} \bm{H}_{k} \hat{\bm{\Sigma}}_{k\vert k-1}$$ (33)

$$\hat{\bm{\Sigma}}_{k+1\vert k} = \bm{F}_{k} \hat{\bm{\Sigma}}_{k\vert k} \bm{F}_{k}^\dag + \bm{G}_{k} \bm{\Sigma}_{\bm{w}_k} \bm{G}_{k}^\dag$$ (34)

初期値

$$\hat{\bm{x}}_{0\vert-1} = \bar{\bm{x}}_0 , \quad \hat{\bm{\Sigma}}_{0\vert-1} = \bm{\Sigma}_{\bm{x}_0}$$ (35)

通常のカルマンフィルタとの違いは各ベクトルと行列の成分が複素数になっている事だけである。そのため転置行列はエルミート行列 で置き換えた。

4.3.5 外部入力を持つカルマンフィルタ

図 9: 外部入力を持つ状態空間のブロック線図。

フィルタ方程式

$$\hat{\bm{x}}_{k\vert k} = \hat{\bm{x}}_{k\vert k-1} + \bm{K}_{k} (\bm{y}_{k} - \bm{H}_{k} \hat{\bm{x}}_{k\vert k-1})$$ (36)

$$\hat{\bm{x}}_{k+1\vert k} = \bm{F}_{k} \hat{\bm{x}}_{k\vert k} + \bm{D}_{k} \bm{u}_{k}$$ (37)

カルマンゲイン

$$\bm{K}_{k} = \hat{\bm{\Sigma}}_{k\vert k-1} \bm{H}_{k}^T (\bm{H}_{k} \hat{\bm{\Sigma}}_{k\vert k-1} \bm{H}_{k}^T + \bm{\Sigma}_{\bm{v}_k})^{-1}$$ (38)

誤差の共分散行列方程式

$$\hat{\bm{\Sigma}}_{k\vert k} = \hat{\bm{\Sigma}}_{k\vert k-1} - \bm{K}_{k} \bm{H}_{k} \hat{\bm{\Sigma}}_{k\vert k-1}$$ (39)

$$\hat{\bm{\Sigma}}_{k+1\vert k} = \bm{F}_{k} \hat{\bm{\Sigma}}_{k\vert k} \bm{F}_{k}^T + \bm{G}_{k} \bm{\Sigma}_{\bm{w}_k} \bm{G}_{k}^T$$ (40)

初期値

$$\hat{\bm{x}}_{0\vert-1} = \bar{\bm{x}}_0 , \quad \hat{\bm{\Sigma}}_{0\vert-1} = \bm{\Sigma}_{\bm{x}_0}$$ (41)

図 10: 外部入力を持つKalmanフィルタのブロック線図。

4.3.6 雑音が相関をもつカルマンフィルタ

フィルタ方程式

$$\hat{\bm{x}}_{k\vert k} = \hat{\bm{x}}_{k\vert k-1} + \bm{K}_{k} (\bm{y}_{k} - \bm{H}_{k} \hat{\bm{x}}_{k\vert k-1})$$ (42)

$$\hat{\bm{x}}_{k+1\vert k} = (\bm{F}_{k} - \bm{G}_{k} \bm{S}_{k} \bm{\Sigma}_{\bm{v}_k}^{-1} \... ...{x}}_{k\vert k} + \bm{G}_{k} \bm{S}_{k} \bm{\Sigma}_{\bm{v}_k}^{-1} \bm{y}_{k}$$ (43)

カルマンゲイン

$$\bm{K}_{k} = \hat{\bm{\Sigma}}_{k\vert k-1} \bm{H}_{k}^T (\bm{H}_{k} \hat{\bm{\Sigma}}_{k\vert k-1} \bm{H}_{k}^T + \bm{\Sigma}_{\bm{v}_k})^{-1}$$ (44)

誤差の共分散行列方程式

$$\hat{\bm{\Sigma}}_{k\vert k} = \hat{\bm{\Sigma}}_{k\vert k-1} - \bm{K}_{k} \bm{H}_{k} \hat{\bm{\Sigma}}_{k\vert k-1}$$ (45)

$$\hat{\bm{\Sigma}}_{k+1\vert k} = (\bm{F}_{k} - \bm{G}_{k} \bm{S}_{k} \bm{\Sigma}_{\bm{v}_k}^{-1} \... ...} (\bm{F}_{k} - \bm{G}_{k} \bm{S}_{k} \bm{\Sigma}_{\bm{v}_k}^{-1} \bm{H}_{k})^T$$

$$+ \bm{G}_{k} (\bm{\Sigma}_{\bm{v}_k} - \bm{S}_{k} \bm{\Sigma}_{\bm{v}_k}^{-1} \bm{S}_{k}^T ) \bm{G}_{k}^T$$ (46)

初期値

$$\hat{\bm{x}}_{0\vert-1} = \bar{\bm{x}}_0 , \quad \hat{\bm{\Sigma}}_{0\vert-1} = \bm{\Sigma}_{\bm{x}_0}$$ (47)

4.3.7 拡張カルマンフィルタ

非線形確率システムに対する準最適フィルタ。

フィルタ方程式

$$\hat{\bm{x}}_{k\vert k} = \hat{\bm{x}}_{k\vert k-1} + \bm{K}_{k} (\bm{y}_{k} - \bm{h}_{k} \hat{\bm{x}}_{k\vert k-1})$$ (48)

$$\hat{\bm{x}}_{k+1\vert k} = \bm{f}_{k} (\hat{\bm{x}}_{k\vert k})$$ (49)

カルマンゲイン

$$\bm{K}_{k} = \hat{\bm{\Sigma}}_{k\vert k-1} \bm{H}_{k}^T (\bm{H}_{k} \hat{\bm{\Sigma}}_{k\vert k-1} \bm{H}_{k}^T + \bm{\Sigma}_{\bm{v}_k})^{-1}$$ (50)

誤差の共分散行列方程式

$$\hat{\bm{\Sigma}}_{k\vert k} = \hat{\bm{\Sigma}}_{k\vert k-1} - \bm{K}_{k} \bm{H}_{k} \hat{\bm{\Sigma}}_{k\vert k-1}$$ (51)

$$\hat{\bm{\Sigma}}_{k+1\vert k} = \bm{F}_{k} \hat{\bm{\Sigma}}_{k\vert k} \bm{F}_{k}^T + \bm{G}_{k} \bm{\Sigma}_{\bm{w}_k} \bm{G}_{k}^T$$ (52)

初期値

$$\hat{\bm{x}}_{0\vert-1} = \bar{\bm{x}}_0 , \quad \hat{\bm{\Sigma}}_{0\vert-1} = \bm{\Sigma}_{\bm{x}_0}$$ (53)