どすえのブログ

京都在住プログラマーの開発ブログ。バイクとキャンプが趣味。

非線形カルマンフィルタ (1) - 線形カルマンフィルタ -

目次

本シリーズについて

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

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

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

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:1段先予測値

 
\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 としてステップ2に戻る.

カルマンスムーザ


\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 ]

としてシミュレーションを行います. 衛星の姿勢角推定の結果を下図に示します. f:id:dosuex:20210516124654p:plain

緑線のフィルタ推定値は観測値の影響を大きく受けていますが, 赤線のスムージング推定値は姿勢角の真の動きに迫っている様子が見えます.

参考資料