Qiita Teams that are logged in
You are not logged in to any team

Log in to Qiita Team
Community
OrganizationAdvent CalendarQiitadon (β)
Service
Qiita JobsQiita ZineQiita Blog
Help us understand the problem. What is going on with this article?

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

本シリーズについて

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

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

TL;DR

コード( Jupyter notebook )はこちらです
https://github.com/Kosuke-Szk/nonlinear_kasmanfilter/blob/master/ex2.ipynb

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

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

としてシミュレーションを行います. 衛星の姿勢角推定の結果を下図に示します.
ex2_image.png

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

コード( Jupyter notebook )はこちらです
https://github.com/Kosuke-Szk/nonlinear_kasmanfilter/blob/master/ex2.ipynb

もう少し詳しい話

Coming soon

Kosuke-Szk
Plasma physics
sciseed
AI・RPAを活用した業務効率化ソリューション『sAI Chat』『sAI Search』を提供するITベンチャー
https://saichat.jp
Why not register and get more from Qiita?
  1. We will deliver articles that match you
    By following users and tags, you can catch up information on technical fields that you are interested in as a whole
  2. you can read useful information later efficiently
    By "stocking" the articles you like, you can search right away