カルマンフィルタとは
一言で言えば、「センサなどで観測できる値と数式で記述された状態と観測値の関係から観測できない状態を推定する方法」である。
線形カルマンフィルタの問題
次の運動方程式および観測方程式に従って状態が遷移し観測されるとする。
\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}_{k}\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$を求めることがカルマンフィルタの問題設定である。これは最小分散推定量とも呼ばれる確率論での基本問題である。
カルマンフィルタの目的は推定値を求めることだが、実際には推定分布を求めるアルゴリズムとなっている。
線形カルマンフィルタの導出
分布が観測値によって更新されるような逐次則を求めるために、推定量の分布の仮定、確率の基本法則と分布の法則を上記の方程式に適用する。
まず、初期の状態の推定分布を次のような正規分布だと仮定する。
\hat{\mathbf{x}}_{0|0} \sim \mathcal{N}(\bar{\mathbf{x}}_{0|0},\mathbf{P}_{0|0})
ただし、
- $\bar{\mathbf{x}}_{n|m}$：mステップ目までの情報で推定したnステップ目の推定量
- $\mathbf{P}_{n|m}$：mステップ目までの情報で推定したnステップ目の推定分布の分散
とする。
◼︎ベイズの定理による導出
観測値列$\mathcal{Y}_{k+1}$が得られた時の状態の推定分布はベイズの式から次のように変形できる。
\begin{align}
p(\hat{\mathbf{x}}_{k+1}|\mathcal{Y}_{k+1})&=p(\hat{\mathbf{x}}_{k+1},\mathcal{Y}_{k+1})/p(\mathcal{Y}_{k+1})\\
&=p(\hat{\mathbf{x}}_{k+1},\mathbf{y}_{k+1},\mathcal{Y}_{k})/p(\mathcal{Y}_{k+1})\\
&=p(\mathbf{y}_{k+1}|\hat{\mathbf{x}}_{k+1},\mathcal{Y}_{k})p(\hat{\mathbf{x}}_{k+1},\mathcal{Y}_{k})/p(\mathcal{Y}_{k+1})\\
&=p(\mathbf{y}_{k+1}|\hat{\mathbf{x}}_{k+1},\mathcal{Y}_{k})p(\hat{\mathbf{x}}_{k+1}|\mathcal{Y}_{k})p(\mathcal{Y}_{k})/p(\mathcal{Y}_{k+1})\\
&=p(\mathbf{y}_{k+1}|\hat{\mathbf{x}}_{k+1})p(\hat{\mathbf{x}}_{k+1}|\mathcal{Y}_{k})p(\mathcal{Y}_{k})/p(\mathcal{Y}_{k+1})
\end{align}
ただし、最後の変形では現在の観測値はそれ以前の観測値に依存しないことを利用した。
また、1ステップ前までの観測値を得たときの現在状態の確率は、1ステップ前の推定分布に基づく予測にほかならないので、状態方程式より各確率は、
\begin{align}
p(\hat{\mathbf{x}}_{k+1}|\mathcal{Y}_{k})
&= \mathcal{N}(\mathbf{F}_{k}\bar{\mathbf{x}}_{k|k}+\mathbf{u}_{k}, \mathbf{F}_{k}\mathbf{P}_{k|k}\mathbf{F}_{k}^{\intercal}+\mathbf{G}_{k}\mathbf{Q}_{k}\mathbf{G}_{k}^{\intercal})\\
&=\mathcal{N}(\hat{\mathbf{x}}_{k+1|k},\mathbf{P}_{k+1|k}) \tag{予測分布}\\
p(\mathbf{y}_{k}|\hat{\mathbf{x}}_{k})
&=\mathcal{N}(\mathbf{H}_{k}\hat{\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})
&\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})^{\intercal}\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})^{\intercal}\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})^{\intercal}\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})^{\intercal}\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})^{\intercal}\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})^{\intercal}\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}))^{\intercal}\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})^{\intercal}\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})^{\intercal}\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})^{\intercal}\mathbf{H}_{k}^{\intercal}\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})^{\intercal}\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})^{\intercal}\mathbf{P}_{k+1|k}^{\mathrm{-1}}(\mathbf{x}_{k+1}-\hat{\mathbf{x}}_{k+1|k})\\
&=\mathbf{x}_{k+1}^{\intercal}\left\{ \mathbf{P}_{k+1|k}+\mathbf{H}_{k}^{\intercal}\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})^{\intercal}\mathbf{R}_{k}^{\mathrm{-1}}\mathbf{H}_{k}+\hat{\mathbf{x}}_{k+1|k}^{\intercal}(\mathbf{P}_{k+1|k}^{\mathrm{-1}}+\mathbf{H}_{k}^{\intercal}\mathbf{R}_{k}^{\mathrm{-1}}\mathbf{H}_{k})\right\}\mathbf{x}_{k+1}+(\mathbf{x}_{k+1}に関係ない項)
\\
&=\mathbf{x}_{k+1}^{\intercal}\left\{ \mathbf{P}_{k+1|k}+\mathbf{H}_{k}^{\intercal}\mathbf{R}_{k}^{\mathrm{-1}}\mathbf{H}_{k}\right\}\mathbf{x}_{k+1}-2\left\{
\mathbf{y}_{k+1}^{\intercal}\mathbf{R}_{k}^{\mathrm{-1}}\mathbf{H}_{k}+\hat{\mathbf{x}}_{k+1|k}^{\intercal}\mathbf{P}_{k+1|k}^{\mathrm{-1}}\right\}\mathbf{x}_{k+1}+(\mathbf{x}_{k+1}に関係ない項)\\
&=(\mathbf{x}_{k+1}-
\left\{
\mathbf{P}_{k+1|k}+\mathbf{H}_{k}^{\intercal}\mathbf{R}_{k}^{\mathrm{-1}}\mathbf{H}_{k}
\right\}^{\mathrm{-1}}
\left\{
\mathbf{y}_{k+1}^{\intercal}\mathbf{R}_{k}^{\mathrm{-1}}\mathbf{H}_{k}+\hat{\mathbf{x}}_{k+1|k}^{\intercal}\mathbf{P}_{k+1|k}^{\mathrm{-1}}\right\})^{\intercal}
\left\{ \mathbf{P}_{k+1|k}+\mathbf{H}_{k}^{\intercal}\mathbf{R}_{k}^{\mathrm{-1}}\mathbf{H}_{k}\right\}(\mathbf{x}_{k+1}-
\left\{
\mathbf{P}_{k+1|k}+\mathbf{H}_{k}^{\intercal}\mathbf{R}_{k}^{\mathrm{-1}}\mathbf{H}_{k}
\right\}^{\mathrm{-1}}
\left\{
\mathbf{y}_{k+1}^{\intercal}\mathbf{R}_{k}^{\mathrm{-1}}\mathbf{H}_{k}+\hat{\mathbf{x}}_{k+1|k}^{\intercal}\mathbf{P}_{k+1|k}^{\mathrm{-1}}\right\})+(\mathbf{x}_{k+1}に関係ない項)
\end{align}
よって、
\begin{align}
\mathbf{P}_{k+1|k+1}
&=\left\{ \mathbf{P}_{k+1|k}+\mathbf{H}_{k}^{\intercal}\mathbf{R}_{k}^{\mathrm{-1}}\mathbf{H}_{k}\right\}^{\mathrm{-1}}
=\mathbf{P}_{k+1|k} - \mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\intercal}(\mathbf{R}_{k} + \mathbf{H}_{k} \mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\intercal})^{\mathrm{-1{}}}\mathbf{H}_{k}\mathbf{P}_{k+1|k}\\
&=(\mathbf{1} - \underbrace{\mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\intercal}(\mathbf{R}_{k} + \mathbf{H}_{k} \mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\intercal})^{\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}^{\intercal}\mathbf{R}_{k}^{\mathrm{-1}}\mathbf{H}_{k}\right\}^{\mathrm{-1}}
\left\{
\mathbf{H}_{k}^{\intercal} \mathbf{R}_{k}^{-\intercal}\mathbf{y}_{k+1}+\mathbf{P}_{k+1|k}^{-\intercal}\hat{\mathbf{x}}_{k+1|k}\right\}
\\
&=\left\{\mathbf{1}- \mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\intercal}(\mathbf{R}_{k} + \mathbf{H}_{k} \mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\intercal})^{\mathrm{-1{}}}\mathbf{H}_{k}\right\}\mathbf{P}_{k+1|k}\left\{
\mathbf{H}_{k}^{\intercal} \mathbf{R}_{k}^{-\intercal}\mathbf{y}_{k+1}+\mathbf{P}_{k+1|k}^{-\intercal}\hat{\mathbf{x}}_{k+1|k}\right\}\\
&=\left\{\mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\intercal}\mathbf{R}_{k}^{\mathrm{-1}} - \mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\intercal}(\mathbf{R}_{k} + \mathbf{H}_{k} \mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\intercal})^{\mathrm{-1{}}}\mathbf{H}_{k}\mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\intercal}\mathbf{R}_{k}^{\mathrm{-1}} \right\}
\mathbf{y}_{k+1}
+\hat{\mathbf{x}}_{k+1|k}+\mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\intercal}(\mathbf{R}_{k} + \mathbf{H}_{k} \mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\intercal})^{\mathrm{-1{}}}\mathbf{H}_{k}\hat{\mathbf{x}}_{k+1|k}\\
&=\left\{\mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\intercal}\mathbf{R}_{k}^{\mathrm{-1}} - \mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\intercal}\mathbf{R}_{k}^{\mathrm{-1}}\mathbf{R}_{k}(\mathbf{R}_{k} + \mathbf{H}_{k} \mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\intercal}\mathbf{R}_{k}^{\mathrm{-1}}\mathbf{R}_{k})^{\mathrm{-1}}\mathbf{H}_{k}\mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\intercal}\mathbf{R}_{k}^{\mathrm{-1}} \right\}
\mathbf{y}_{k+1}
+\hat{\mathbf{x}}_{k+1|k}+\mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\intercal}(\mathbf{R}_{k} + \mathbf{H}_{k} \mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\intercal})^{\mathrm{-1{}}}\mathbf{H}_{k}\hat{\mathbf{x}}_{k+1|k}\\
&=\left\{
(\mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\intercal}\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}
+\hat{\mathbf{x}}_{k+1|k}+\mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\intercal}(\mathbf{R}_{k} + \mathbf{H}_{k} \mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\intercal})^{\mathrm{-1{}}}\mathbf{H}_{k}\hat{\mathbf{x}}_{k+1|k}\\
&=\left\{(
\mathbf{R}_{k}+\mathbf{H}_{k}\mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\intercal})\mathbf{H}_{k}^{\mathrm{-T}}\mathbf{P}_{k+1|k}^{\mathrm{-1}}
\right\}^{\mathrm{-1}}
\mathbf{y}_{k+1}
+\hat{\mathbf{x}}_{k+1|k}+\mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\intercal}(\mathbf{R}_{k} + \mathbf{H}_{k} \mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\intercal})^{\mathrm{-1{}}}\mathbf{H}_{k}\hat{\mathbf{x}}_{k+1|k}\\
&=\underbrace{\mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\intercal}(\mathbf{R}_{k} + \mathbf{H}_{k} \mathbf{P}_{k+1|k}\mathbf{H}_{k}^{\intercal})^{\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の恒等式を利用した。上記の更新式をアルゴリズムにまとめたものがカルマンフィルタとなる。
◼最適化に基づく導出
状態方程式と観測方程式を確率変数の期待値で表現した場合、
\begin{align}
\mathbf{x}_{k+1}-\left(\mathbf{F}_{k}\mathbf{x}_{k}+\mathbf{u}_{k}+\mathbf{G}_{k}\mathbf{w}_{k}\right)&=\mathbf{0}\\
\Rightarrow \mathbf{x}_{k+1}-\left(\mathbf{F}_{k}\bar{\mathbf{x}}_{k}+\mathbf{u}_{k}\right)&=\mathbf{\epsilon}_{s}\\
\mathbf{y}_{k}-\left(\mathbf{H}_{k}\mathbf{x}_{k}+\mathbf{v}_{k}\right)&=\mathbf{0}\\
\Rightarrow \mathbf{y}_{k+1}-\mathbf{H}_{k}\mathbf{x}_{k+1}&=\mathbf{\epsilon}_{o}\\
\end{align}
という変形が可能である。各残差項$\mathbf{\epsilon}_{*}$は次のような分散を持つ。
\begin{align}
\mathbb{V}[\mathbf{\epsilon}_{s}]&=\mathbf{F}_{k}\mathbf{P}_{k|k}\mathbf{F}_{k}^{\intercal}+\mathbf{G}_{k}\mathbf{Q}_{k}\mathbf{G}_{k}^{\intercal}\\
\mathbb{V}[\mathbf{\epsilon}_{o}]&=\mathbf{R}_{k}
\end{align}
分散が大きいモデル式は信頼できないということなので、共分散行列の逆行列を重みに取った次のような最適化を考える。
\begin{align}
\underset{\mathbf{x}_{k+1}}{\mathrm{argmin}~}&
\|\mathbf{\epsilon}_{s}\|^{2}_{\mathbb{V}[\mathbf{\epsilon}_{s}]^{-1}}+\|\mathbf{\epsilon}_{o}\|^{2}_{\mathbb{V}[\mathbf{\epsilon}_{o}]^{-1}}\\
\Leftrightarrow
\underset{\mathbf{x}_{k+1}}{\mathrm{argmin}~}&
\|\mathbf{x}_{k+1}-\left(\mathbf{F}_{k}\bar{\mathbf{x}}_{k}+\mathbf{u}_{k}\right)\|^{2}_{\left(\mathbf{F}_{k}\mathbf{P}_{k|k}\mathbf{F}_{k}^{\intercal}+\mathbf{G}_{k}\mathbf{Q}_{k}\mathbf{G}_{k}^{\intercal}\right)^{-1}}+\|\mathbf{y}_{k+1}-\mathbf{H}_{k}\mathbf{x}_{k+1}\|^{2}_{\mathbf{R}_{k}^{-1}}
\end{align}
この解は次のようになる。
\begin{align}
\mathbf{x}_{k+1} &= \left(\left(\mathbf{F}_{k}\mathbf{P}_{k|k}\mathbf{F}_{k}^{\intercal}+\mathbf{G}_{k}\mathbf{Q}_{k}\mathbf{G}_{k}^{\intercal}\right)^{-1}
+\mathbf{H}_{k}^{\intercal}\mathbf{R}_{k}^{-1}\mathbf{H}_{k}\right)^{-1} \left(\mathbf{R}_{k}^{-1}\mathbf{H}_{k}^{\intercal}\mathbf{y}_{k+1}+
\left(\mathbf{F}_{k}\mathbf{P}_{k|k}\mathbf{F}_{k}^{\intercal}+\mathbf{G}_{k}\mathbf{Q}_{k}\mathbf{G}_{k}^{\intercal}\right)^{-1}\left(\mathbf{F}_{k}\bar{\mathbf{x}}_{k}+\mathbf{u}_{k}\right)\right)\\
&= \left(\mathbf{P}_{k+1|k}^{-1}
+\mathbf{H}_{k}^{\intercal}\mathbf{R}_{k}^{-1}\mathbf{H}_{k}\right)^{-1} \left(\mathbf{R}_{k}^{-1}\mathbf{H}_{k}^{\intercal}\mathbf{y}_{k+1}+
\mathbf{P}_{k+1|k}^{-1}\hat{\mathbf{x}}_{k+1|k}\right)
\end{align}
先ほどと同じ変形から、カルマンフィルターの推定値が求まる。
非線形カルマンフィルタ
非線形カルマンフィルタは線形カルマンフィルタの枠組みを非線形モデルに対して使った方法である。線形カルマンフィルタは「正規分布を線形モデルで更新したものはまた正規分布であること」を存分に利用していた。一方で、非線形モデルで更新された正規分布は一般には正規分布ではない。線形カルマンフィルタの枠組みを利用するには非線形モデルで更新された分布（上記式で「予測分布」と記述したもの）をなんとかして正規分布として近似する必要がある。正規分布への近似の仕方で非線形カルマンフィルタにはいくつか種類がある。
下記はその一例である。
- Extended Kalman Filter （EKF：拡張カルマンフィルタ）：前ステップの推定値まわりで局所線形化したモデルで前ステップの推定分布を更新した正規分布で近似
- Unscented Kalman Filter （UKF：無香料カルマンフィルタ）：前ステップの予測分布からサンプリングしたシグマ点を非線形モデルで更新した点群の平均・分散を持つ正規分布で近似
それぞれの正規分布への近似の仕方を図で表現すると、次のようになる。
図からわかるように、EKFの近似は非線形性が強い場合に非線形モデルによる更新分布から大きくずれることがある。これは推定値まわりの勾配を利用しているために周辺の非線形を考慮できていないからである。それに対してUKFの近似は推定値周辺の情報をサンプリングによってうまく取り込むこと非線形性が強い場合でも比較的よく近似することができる。
まとめ
- 正規分布を用いたカルマンフィルタの導出を行った
- 正規分布への近似という観点で非線形カルマンフィルタを分類した