本シリーズについて

非線形現象に対するカルマンフィルタを本で勉強したのでメモも兼ねてまとめます.

観測データに基づいて, 線形確率システムの状態ベクトルを逐次的に最適化・推定するアルゴリズムとして, カルマンフィルタが有名です. カルマンフィルタは状態ベクトルの時間発展と, 状態ベクトルの観測に線形性を仮定しているため, 非線形の現象に適用するためには工夫が必要となります. 本シリーズでは線形カルマンフィルタから順を追って非線形カルマンフィルタの実装へ進んでいきます.

非線形モデルに拡張したアンサンブルカルマンフィルタはこちら↓ dosuex.hatenablog.com

今回は状態遷移、観測がともに線形である(行列演算で記述できる)ケースを考えます.

TL;DR

コード( Jupyter notebook )はこちらです.

github.com

状態空間モデルで表される線形確率システム

$\begin{align} x_{t+1} &= F_t x_t + G_t w_t \\ y_t &= H_tx_t + v_t\\ \end{align}$

を考えます。ここで, $x_t\in \mathbb{R}^n$ は状態ベクトル, $y_t \in \mathbb{R}^p$ は観測ベクトル, $w_t \in \mathbb{R}^m$ , $v_t \in \mathbb{R}^p$ はガウスノイズベクトルです. また $F_t \in \mathbb{R}^{n\times n}$ は遷移行列, $G_t \in \mathbb{R}^{n \times m}$ は駆動行列, $H_t \in \mathbb{R}^{p \times n}$ は観測行列と呼ばれます. さらに, $x_t$ , $w_t$ , $v_t$ それぞれに対する誤差共分散行列として $P,Q,R$ を導入します.

取り急ぎ, 下式たちを実装すればカルマンフィルタができます. 添字の $t / t-1$ は時刻 $t-1$ の情報を用いて時刻 $t$ の状態を推定する操作(予測)を表し, $t/t$ は現在の時刻の情報を用いて状態を最適化する操作(フィルタリング)を表します. また $t/N$ は時刻 $N$ まで予測が完了している状態での時刻 $t$ の最適化操作(スムージング)を表します.

カルマンフィルタ

1：初期値を $\hat{x}_{0/-1}=\bar{x}_0$ , $P_{0/-1}=P_0$ とおき, $t=0$ とする.
2：観測更新ステップ

　A：カルマンゲイン

$K_t = P_{t/t-1}H_t^T [H_t P_{t/t-1} H_t^T + R_t]^{-1}$

　B：フィルタ推定値

$\hat{x}_{t/t} = \hat{x}_{t/t-1} + K_t [y_t - H_t \hat{x}_{t/t-1}]$

　C：フィルタ推定誤差共分散行列

$P_{t/t} = P_{t/t-1} - K_t H_t P_{t/t-1}$

3：時間更新ステップ

　A：１段先予測値

$\hat{x}_{t+1/t} = F_t \hat{x}_{t/t}$

　B：予測誤差共分散行列

$P_{t+1/t} = F_t P_{t/t} F_t^T + G_t Q_t G_t^T$

4： $t \leftarrow t+1$ としてステップ２に戻る.

カルマンスムーザ

$\begin{align} \hat{x}_{t/N} &= \hat{x}_{t/t} + C_t(\hat{x}_{t+1/N} - \hat{x}_{t+1/t})\\ C_t &= P_{t/t} F_t^T P_{t+1/t}^{-1} \\ P_{t/N} &= P_{t/t} + C_t [P_{t+1/N}-P_{t+1/t}]C_t^T \end{align}$

実装

動的な線形システムの逐次予測をカルマンフィルタを使って行います. 参考資料のp.56にある例の通り, 人工衛星の回転運動を線形近似した4次元システムを考えます.

$\begin{align} \dot{x}_t^{(1)} &= x_t^{(2)}\\ \dot{x}_t^{(2)} &= x_t^{(3)} + x_t^{(4)}\\ \dot{x}_t^{(3)} &= 0\\ \dot{x}_t^{(4)} &= -0.5 x_t^{(4)} + \xi_t \end{align}$

ただし, ${x}_t^{(1)}$ : 人工衛星の姿勢角度, ${x}_t^{(2)}$ : 角速度, ${x}_t^{(3)}$ : 角加速度の平均値成分, ${x}_t^{(4)}$ : 角加速度のランダム成分であり, $\xi_t$ は平均0のガウスノイズです. サンプル間隔 $\Delta =1.0$ で離散化すると時間発展は以下で記述されます.

$x_{t+1} = \left( \begin{array}{ccc} 1 & 1 & 0.5 & 0.5\\ 0 & 1 & 1 & 1\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 0.606 \end{array} \right) x_t + \left( \begin{array}{ccc} 0\\ 0\\ 0\\ 1 \end{array} \right) w_t$

ただし, ガウスノイズ $w_t, v_t$ の分散は $Q=0.64\times 10^{-2}$ , $R=1$ とします. 状態ベクトルの初期値および推定値の初期値を

$x_0 = \left(\begin{array}{ccc} 1.25\\ 0.06\\ 0.01\\ -0.003 \end{array} \right), \hat{x}_{0/-1} = \left(\begin{array}{ccc} 0\\ 0\\ 0\\ 0 \end{array} \right), P_{0/-1} = \rm{diag}[10,10,10,10$ ]