LoginSignup
2
3
@categoryik

Kalman Filter入門

Last updated at Posted at 2023-11-26

カルマンフィルタとは

一言で言えば、「センサなどで観測できる値と数式で記述された状態と観測値の関係から観測できない状態を推定する方法」である。

カルマンフィルタとその導出

本記事ではカルマンフィルタを確率論から導出する。

線形カルマンフィルタの問題

次のような運動方程式および観測方程式に従って状態遷移し観測されると考える。

\begin{align}
\mathbf{x}_{k+1}&=\mathbf{F}_{k}\mathbf{x}_{k}+\mathbf{u}_{k}+\mathbf{G}_{k}\mathbf{w}_{k} &&  (状態方程式)\\
\mathbf{y}_{k}&=\mathbf{H}_{i}\mathbf{x}_{k}+\mathbf{v}_{k}
\end{align}

ただし、

  • $\mathbf{x}_{k}$：推定対象の状態
  • $\mathbf{y}_{k}$：観測可能な値
  • $\mathbf{F}_{k}$：線形遷移モデル
  • $\mathbf{H}_{k}$：線形観測モデル
  • $\mathbf{w}_{k}$：正規分布を仮定したシステムノイズ $~\mathcal{N}(\mathbf{0},\mathbf{Q}_k)$
  • $\mathbf{v}_{k}$：正規分布を仮定した観測ノイズ $~\mathcal{N}(\mathbf{0},\mathbf{R}_k)$
  • $\mathbf{G}_{k}$：観測ノイズの変換行列

である。この方程式で次の状態は現在の情報からのみ決まる、つまり、マルコフ過程である。

この時、観測値列

\mathcal{Y}_{k} = \mathbf{y}_1,\mathbf{y}_2,...,\mathbf{y}_k

が得られた時の$\mathbf{x}_{k}$の確率$p(\mathbf{x}_k|\mathcal{Y}_k)$に関する
分散の期待値$E[(\mathbf{x}_k-\hat{\mathbf{x}}_k)^2]$を最小にする推定量$\hat{\mathbf{x}}_k$を求めることがカルマンフィルタの問題設定である。これは最小分散推定量とも呼ばれる。

上記のように、カルマンフィルタの目的は推定値を求めることだが、実際には推定分布を求めるアルゴリズムとなっている。

線形カルマンフィルタの導出

分布が観測値によって更新されるような逐次則を求めるために、推定量の分布の仮定、確率の基本法則と分布の法則を上記の方程式に適用する。

まず、初期の状態分布を次のような正規分布だと仮定する。

\mathcal{N}(\hat{\mathbf{x}}_{0|0},\mathbf{P}_{0|0})

ただし、

  • $\hat{\mathbf{x}}_{n|m}$：mステップ目までの情報で推定した推定量
  • $\mathbf{P}_{n|m}$：mステップ目までの情報で推定したnステップ目の推定分布の分散

とする。

この時の推定量の予測値および観測値は運動方程式と正規分布の再生性から正規分布となる。

ところで、観測値列$\mathcal{Y}_{k+1}$が得られた時の分布はベイズの式から次のように変形できる。

\begin{align}
p(\mathbf{x}_{k+1}|\mathcal{Y}_{k+1})&=p(\mathbf{x}_{k+1},\mathcal{Y}_{k+1})/p(\mathcal{Y}_{k+1})\\
&=p(\mathbf{x}_{k+1},\mathbf{y}_{k+1},\mathcal{Y}_{k})/p(\mathcal{Y}_{k+1})\\
&=p(\mathbf{y}_{k+1}|\mathbf{x}_{k+1},\mathcal{Y}_{k})p(\mathbf{x}_{k+1},\mathcal{Y}_{k})/p(\mathcal{Y}_{k+1})\\
&=p(\mathbf{y}_{k+1}|\mathbf{x}_{k+1},\mathcal{Y}_{k})p(\mathbf{x}_{k+1}|\mathcal{Y}_{k})p(\mathcal{Y}_{k})/p(\mathcal{Y}_{k+1})
\end{align}

運動方程式より観測値はそれ以前の観測値に依存しないので、

p(\mathbf{y}_{k+1}|\mathbf{x}_{k+1},\mathcal{Y}_{k})=p(\mathbf{y}_{k+1}|\mathbf{x}_{k+1})

である。

また、1ステップ前までの観測値を得たときの現在状態の確率は１ステップ前の推定分布を利用した予測と同じである。よって、

p(\mathbf{x}_{k+1}|\mathcal{Y}_{k})=p(\mathbf{x}_{k+1}|\mathbf{x}_{k}\sim\mathcal{N}(\hat{\mathbf{x}}_{k|k},\mathbf{P}_{k|k}))

と書き換えられる。

上記をまとめると、ベイズの式は、

\begin{align}
p(\mathbf{x}_{k+1}|\mathcal{Y}_{k+1})
&=p(\mathbf{y}_{k+1}|\mathbf{x}_{k+1})p(\mathbf{x}_{k+1}|\hat{\mathbf{x}}_{k|k})p(\mathcal{Y}_{k-1})/p(\mathcal{Y}_k)
\end{align}

と書ける。これは正規分布になっているので、指数項の中身さえ整理できれば分布の期待値と分散を計算できる。

状態方程式より各確率は、

\begin{align}
p(\mathbf{x}_{k+1}|\mathbf{x}_{k}\sim\mathcal{N}(\hat{\mathbf{x}}_{k|k},\mathbf{P}_{k|k}))
&= \mathcal{N}(\mathbf{F}_{k}\hat{\mathbf{x}}_{k|k}, \mathbf{F}_{k}\mathbf{P}_{k|k}\mathbf{F}_{k}^{\mathrm{T}}+\mathbf{G}_{k}\mathbf{Q}_{k}\mathbf{G}_{k}^{\mathrm{T}})\\
&=\mathcal{N}(\hat{\mathbf{x}}_{k+1|k},\mathbf{P}_{k+1|k}) \tag{予測分布}\\
p(\mathbf{y}_{k}|\mathbf{x}_{k})
&=\mathcal{N}(\mathbf{H}_{k}\mathbf{x}_{k}, \mathbf{R}_{k})　\tag{観測分布}
\end{align}

という関係を持つ。また、

p(\mathcal{Y}_{k-1})/p(\mathcal{Y}_k)

は$\mathbf{x}_{k}$に関係のない定数項であったので、

\begin{align}
p(\hat{\mathbf{x}}_{k+1|k+1}|\mathcal{Y}_{k+1})
&=p(\mathbf{y}_{k+1}|\mathbf{x}_{k+1})p(\mathbf{x}_{k+1}|\hat{\mathbf{x}}_{k|k})p(\mathcal{Y}_{k-1})/p(\mathcal{Y}_k)\\
&\propto \mathcal{N}(\mathbf{H}_{k}\mathbf{x}_{k+1}, \mathbf{R}_{k})\mathcal{N}(\hat{\mathbf{x}}_{k+1|k},\mathbf{P}_{k+1|k})\\
&\propto \exp{\left\{(\mathbf{y}_{k+1}-\mathbf{H}_{k}\mathbf{x}_{k+1})^{\mathrm{T}}\mathbf{R}_{k}^{\mathrm{-1}}(\mathbf{y}_{k+1}-\mathbf{H}_{k}\mathbf{x}_{k+1})\right\}}\exp{\left\{(\mathbf{x}_{k+1}-\hat{\mathbf{x}}_{k+1|k})^{\mathrm{T}}\mathbf{P}_{k+1|k}^{\mathrm{-1}}(\mathbf{x}_{k+1}-\hat{\mathbf{x}}_{k+1|k})\right\}}\\
&\propto \exp{\left\{(\mathbf{y}_{k+1}-\mathbf{H}_{k}\mathbf{x}_{k+1})^{\mathrm{T}}\mathbf{R}_{k}^{\mathrm{-1}}(\mathbf{y}_{k+1}-\mathbf{H}_{k}\mathbf{x}_{k+1})+(\mathbf{x}_{k+1}-\hat{\mathbf{x}}_{k+1|k})^{\mathrm{T}}\mathbf{P}_{k+1|k}^{\mathrm{-1}}(\mathbf{x}_{k+1}-\hat{\mathbf{x}}_{k+1|k})\right\}}
\end{align}

となる。ここで、この正規分布の平均と分散を求めるために指数関数の内部を平方完成する。

\begin{align}
&(\mathbf{y}_{k+1}-\mathbf{H}_{k}\mathbf{x}_{k+1})^{\mathrm{T}}\mathbf{R}_{k}^{\mathrm{-1}}(\mathbf{y}_{k+1}-\mathbf{H}_{k}\mathbf{x}_{k+1})+(\mathbf{x}_{k+1}-\hat{\mathbf{x}}_{k+1|k})^{\mathrm{T}}\mathbf{P}_{k+1|k}^{\mathrm{-1}}(\mathbf{x}_{k+1}-\hat{\mathbf{x}}_{k+1|k})\\
&= ((\mathbf{y}_{k+1}-\mathbf{H}_{k}\hat{\mathbf{x}}_{k+1|k})-\mathbf{H}_{k}(\mathbf{x}_{k+1}-\hat{\mathbf{x}}_{k+1|k}))^{\mathrm{T}}\mathbf{R}_{k}^{\mathrm{-1}}((\mathbf{y}_{k+1}-\mathbf{H}_{k}\hat{\mathbf{x}}_{k+1|k})-\mathbf{H}_{k}(\mathbf{x}_{k+1}-\hat{\mathbf{x}}_{k+1|k}))+(\mathbf{x}_{k+1}-\hat{\mathbf{x}}_{k+1|k})^{\mathrm{T}}\mathbf{P}_{k+1|k}^{\mathrm{-1}}(\mathbf{x}_{k+1}-\hat{\mathbf{x}}_{k+1|k})\\
&=(\mathbf{y}_{k+1}-\mathbf{H}_{k}\hat{\mathbf{x}}_{k+1|k})^{\mathrm{T}}\mathbf{R}_{k}^{\mathrm{-1}}(\mathbf{y}_{k+1}-\mathbf{H}_{k}\hat{\mathbf{x}}_{k+1|k})+(\mathbf{x}_{k+1}-\hat{\mathbf{x}}_{k+1|k})^{\mathrm{T}}\mathbf{H}_{k}^{\mathrm{T}}\mathbf{R}_{k}^{\mathrm{-1}}\mathbf{H}_{k}(\mathbf{x}_{k+1}-\hat{\mathbf{x}}_{k+1|k})ー2(\mathbf{y}_{k+1}-\mathbf{H}_{k}\hat{\mathbf{x}}_{k+1|k})^{\mathrm{T}}\mathbf{R}_{k}^{\mathrm{-1}}\mathbf{H}_{k}(\mathbf{x}_{k+1}-\hat{\mathbf{x}}_{k+1|k})+(\mathbf{x}_{k+1}-\hat{\mathbf{x}}_{k+1|k})^{\mathrm{T}}\mathbf{P}_{k+1|k}^{\mathrm{-1}}(\mathbf{x}_{k+1}-\hat{\mathbf{x}}_{k+1|k})\\
&=\mathbf{x}_{k+1}^{\mathrm{T}}\left\{ \mathbf{P}_{k+1|k}+\mathbf{H}_{k}^{\mathrm{T}}\mathbf{R}_{k}^{\mathrm{-1}}\mathbf{H}_{k}\right\}\mathbf{x}_{k+1}-2\left\{ (\mathbf{y}_{k+1}-\mathbf{H}_{k}\hat{\mathbf{x}}_{k+1|k})^{\mathrm{T}}\mathbf{R}_{k}^{\mathrm{-1}}\mathbf{H}_{k}+\hat{\mathbf{x}}_{k+1|k}^{\mathrm{T}}(\mathbf{P}_{k+1|k}^{\mathrm{-1}}+\mathbf{H}_{k}^{\mathrm{T}}\mathbf{R}_{k}^{\mathrm{-1}}\mathbf{H}_{k})\right\}\mathbf{x}_{k+1}+(\mathbf{x}_{k+1}に関係ない項)\\
&=(\mathbf{x}_{k+1}-
\left\{ 
    \mathbf{P}_{k+1|k}+\mathbf{H}_{k}^{\mathrm{T}}\mathbf{R}_{k}^{\mathrm{-1}}\mathbf{H}_{k}
\right\}^{\mathrm{-1}}
\left\{ 
   \mathbf{H}_{k}^{\mathrm{T}}\mathbf{R}_{k}^{\mathrm{-1}} (\mathbf{y}_{k+1}-\mathbf{H}_{k}\hat{\mathbf{x}}_{k+1|k})+(\mathbf{P}_{k+1|k}^{\mathrm{-1}}+\mathbf{H}_{k}^{\mathrm{T}}\mathbf{R}_{k}^{\mathrm{-1}}\mathbf{H}_{k})\hat{\mathbf{x}}_{k+1|k}
\right\})^{\mathrm{T}}
\left\{ \mathbf{P}_{k+1|k}+\mathbf{H}_{k}^{\mathrm{T}}\mathbf{R}_{k}^{\mathrm{-1}}\mathbf{H}_{k}\right\}(\mathbf{x}_{k+1}-
\left\{ 
    \mathbf{P}_{k+1|k}+\mathbf{H}_{k}^{\mathrm{T}}\mathbf{R}_{k}^{\mathrm{-1}}\mathbf{H}_{k}
\right\}^{\mathrm{-1}}
\left\{ 
   \mathbf{H}_{k}^{\mathrm{T}}\mathbf{R}_{k}^{\mathrm{-1}} (\mathbf{y}_{k+1}-\mathbf{H}_{k}\hat{\mathbf{x}}_{k+1|k})+(\mathbf{P}_{k+1|k}^{\mathrm{-1}}+\mathbf{H}_{k}^{\mathrm{T}}\mathbf{R}_{k}^{\mathrm{-1}}\mathbf{H}_{k})\hat{\mathbf{x}}_{k+1|k}
\right\})+(\mathbf{x}_{k+1}に関係ない項)
\end{align}

よって、

\begin{align}
\mathbf{P}_{k+1|k+1}
&=\left\{ \mathbf{P}_{k+1|k}+\mathbf{H}_{k}^{\mathrm{T}}\mathbf{R}_{k}^{\mathrm{-1}}\mathbf{H}_{k}\right\}^{\mathrm{-1}}
=\mathbf{P}_{k+1|k} - \mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\mathrm{T}}(\mathbf{R}_{k} +  \mathbf{H}_{k} \mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\mathrm{T}})^{\mathrm{-1{}}}\mathbf{H}_{k}\mathbf{P}_{k+1|k}\\
&=(\mathbf{1} - \underbrace{\mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\mathrm{T}}(\mathbf{R}_{k} +  \mathbf{H}_{k} \mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\mathrm{T}})^{\mathrm{-1{}}}}_{\mathbf{K_{k}}: 最適カルマンゲイン}\mathbf{H}_{k})\mathbf{P}_{k+1|k}\\
&=(\mathbf{1} - \mathbf{K_{k}}\mathbf{H}_{k})\mathbf{P}_{k+1|k}\\
\hat{\mathbf{x}}_{k+1|k+1}&=\left\{ \mathbf{P}_{k+1|k}+\mathbf{H}_{k}^{\mathrm{T}}\mathbf{R}_{k}^{\mathrm{-1}}\mathbf{H}_{k}\right\}^{\mathrm{-1}}
\left\{ 
   \mathbf{H}_{k}^{\mathrm{T}}\mathbf{R}_{k}^{\mathrm{-1}} (\mathbf{y}_{k+1}-\mathbf{H}_{k}\hat{\mathbf{x}}_{k+1|k})+(\mathbf{P}_{k+1|k}^{\mathrm{-1}}+\mathbf{H}_{k}^{\mathrm{T}}\mathbf{R}_{k}^{\mathrm{-1}}\mathbf{H}_{k})\hat{\mathbf{x}}_{k+1|k}
\right\}\\
&=\left\{\mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\mathrm{T}}\mathbf{R}_{k}^{\mathrm{-1}} - \mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\mathrm{T}}(\mathbf{R}_{k} +  \mathbf{H}_{k} \mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\mathrm{T}})^{\mathrm{-1{}}}\mathbf{H}_{k}\mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\mathrm{T}}\mathbf{R}_{k}^{\mathrm{-1}} \right\}
   (\mathbf{y}_{k+1}-\mathbf{H}_{k}\hat{\mathbf{x}}_{k+1|k})
+\hat{\mathbf{x}}_{k+1|k}\\
&=\left\{\mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\mathrm{T}}\mathbf{R}_{k}^{\mathrm{-1}} - \mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\mathrm{T}}\mathbf{R}_{k}^{\mathrm{-1}}\mathbf{R}_{k}(\mathbf{R}_{k} +  \mathbf{H}_{k} \mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\mathrm{T}}\mathbf{R}_{k}^{\mathrm{-1}}\mathbf{R}_{k})^{\mathrm{-1}}\mathbf{H}_{k}\mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\mathrm{T}}\mathbf{R}_{k}^{\mathrm{-1}} \right\}
   (\mathbf{y}_{k+1}-\mathbf{H}_{k}\hat{\mathbf{x}}_{k+1|k})
+\hat{\mathbf{x}}_{k+1|k}\\
&=\left\{
(\mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\mathrm{T}}\mathbf{R}_{k}^{\mathrm{-1}})^{\mathrm{-1}}+\mathbf{R}_{k}\mathbf{R}_{k}^{\mathrm{-1}}\mathbf{H}_{k}
\right\}^{\mathrm{-1}}
   (\mathbf{y}_{k+1}-\mathbf{H}_{k}\hat{\mathbf{x}}_{k+1|k})
+\hat{\mathbf{x}}_{k+1|k}\\
&=\left\{(
\mathbf{R}_{k}+\mathbf{H}_{k}\mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\mathrm{T}})\mathbf{H}_{k}^{\mathrm{-T}}\mathbf{P}_{k+1|k}^{\mathrm{-1}}
\right\}^{\mathrm{-1}}
   (\mathbf{y}_{k+1}-\mathbf{H}_{k}\hat{\mathbf{x}}_{k+1|k})
+\hat{\mathbf{x}}_{k+1|k}\\
&=\underbrace{\mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\mathrm{T}}(\mathbf{R}_{k} +  \mathbf{H}_{k} \mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\mathrm{T}})^{\mathrm{-1{}}}}_{\mathbf{K_{k}}}
   (\mathbf{y}_{k+1}-\mathbf{H}_{k}\hat{\mathbf{x}}_{k+1|k})
+\hat{\mathbf{x}}_{k+1|k}\\
&=\hat{\mathbf{x}}_{k+1|k}+\mathbf{K_{k}}(\mathbf{y}_{k+1}-\mathbf{H}_{k}\hat{\mathbf{x}}_{k+1|k})\\
\end{align}

ただし、逆行列の変換にはWoodburyの恒等式を利用した。上記の更新式をアルゴリズムにまとめたものがカルマンフィルタとなる。

上記の計算を図にまとめると次のような関係となっている。
image.png

非線形カルマンフィルタ

非線形カルマンフィルタは線形カルマンフィルタの枠組みを非線形モデルに対して使った方法である。線形カルマンフィルタは「更新前の正規分布を線形モデルで更新されたものはまた正規分布であること」を存分に利用していた。一方で、非線形モデルで更新された正規分布は一般には正規分布ではない。線形カルマンフィルタの枠組みを利用するには非線形モデルで更新された分布（上記式で「予測分布」と記述したもの）をなんとかして正規分布として近似する必要がある。正規分布への近似の仕方で非線形カルマンフィルタにはいくつか種類がある。

下記はその一例である。

  • Extended Kalman Filter　（EKF：拡張カルマンフィルタ）：前ステップの推定値まわりで局所線形化したモデルで前ステップの推定分布を更新した正規分布で近似
  • Unscented Kalman Filter　（UKF：無香料カルマンフィルタ）：前ステップの予測分布からサンプリングしたシグマ点を非線形モデルで更新した点群の平均・分散を持つ正規分布で近似

それぞれの正規分布への近似の仕方を図で表現すると、次のようになる。
image.png

図からわかるように、EKFの近似は非線形性が強い場合に非線形モデルによる更新分布から大きくずれることがある。これは推定値での勾配を利用しているために周辺の非線形を考慮できていないためである。それに対してUKFの近似は最低値周辺の情報をサンプリングによってうまく取り込むこと非線形性が強い場合でも比較的よく近似することができる。

まとめ

  • 正規分布を用いたカルマンフィルタの導出を行った
  • 正規分布への近似という観点で非線形カルマンフィルタを分類した

参考文献

2
3
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
2
3