定式化

推定したい状態量の時刻 $t$ での真の値を $\vec{x}_{t}^{true}$ とする。 $\vec{x}_{t}^{true}$ は前周期の値 $\vec{x}_{t-1}^{true}$ をもとにしてある状態量 $\vec{u}_{t}^{true}$ と正規分布に従うノイズ $\vec{w}_{t}$ によって線形で表現できると仮定する。

$\begin{equation} \vec{x}_{t}^{true} = F_{t} \vec{x}_{t-1}^{true} + B_{t} \vec{u}_{t} + \vec{w}_{t} \tag{1} \end{equation}$

このシステムにおいて $\vec{x}_{t}^{true}$ の推定値 $\vec{x}_{t}^{est}$ を求めることを考える。まずオドメトリによる推定値は、前の周期に算出した推定値 $\vec{x}_{t-1}^{est}$ を伝搬させることで求められる。

$\begin{equation} \vec{x}_{t}^{odom} = F_{t}\vec{x}_{t-1}^{est}+B_{t}\vec{u}_{t} \tag{2} \end{equation}$

ここで、

$\begin{eqnarray} \vec{x}_{t}^{odom} - \vec{x}_{t}^{true} &=& (F_{t}\vec{x}_{t-1}^{est} + B_{t}\vec{u}_{t}) - (F_{t}\vec{x}_{t-1}^{true}+B_{t}\vec{u}_{t}+\vec{w}_{t}) \\\ &=& F(\vec{x}_{t-1}^{est} - \vec{x}_{t-1}^{true}) - \vec{w}_{t} \end{eqnarray}$

と表されるので、 $\vec{x}_{t}^{odom}$ の分散共分散行列 $V(\vec{x}_{t}^{odom})$ は

$\begin{equation} V(\vec{x}_{t}^{odom}) = F V(\vec{x}_{t-1}^{est}) F^{T} + V(\vec{w}) \tag{3} \end{equation}$

となる。ここでは測定結果 $\vec{u}_{t}$ に誤差はないと考えていることに注意。もし誤差がある場合この項由来の成分が追加で加わることになる。
次に観測値について考える。観測値 $\vec{z}_{t}$ は真の値 $\vec{x}_{t}^{true}$ を観測による線形変換したものに対して正規分布に従うノイズ成分 $\vec{v}_{t}$ をのせたものだとし、その結果が示す推定値を $\vec{x}_{t}^{obs}$ とすると、

$\begin{equation} \vec{z}_{t} = H_{t}\vec{x}_{t}^{true} + \vec{v}_{t} = H_{t}\vec{x}_{t}^{obs} \tag{4} \end{equation}$

(1)(3)のように線形の関係で表される場合線形カルマンフィルターが使える。オドメトリによる推定値と観測値を使って、最適な推定値 $\vec{x}_{t}^{est}$ はカルマンゲイン $K_{t}$ を使って以下のように表される。

$\begin{equation} \vec{x}_{t}^{est} = \vec{x}_{t}^{odom}+K_{t}(\vec{z}_{t} - H_{t}\vec{x}_{t}^{odom}) = (I-K_{t}H_{t})\vec{x}_{t}^{odom}+K_{t}H_{t}\vec{x}_{t}^{obs} \tag{5} \end{equation}$

オドメトリによる推定値をベースとし、観測値による補正量にカルマンゲインをかけたものを最終的な推定値としていることがわかる。
最後に $\vec{x}_{t}^{est}$ に関する分散共分散行列を考える。

$\begin{equation} \vec{x}_{t}^{est} - \vec{x}_{t}^{true} = (I-K_{t}H_{t})(\vec{x}_{t}^{odom} - \vec{x}_{t}^{true}) + K_{t}H_{t}(\vec{x}_{t}^{obs}- \vec{x}_{t}^{true}) \end{equation}$

なので

$\begin{eqnarray} V(\vec{x}_{t}^{est}) &=& (I-K_{t}H_{t})V(\vec{x}_{t}^{odom}) (I-K_{t}H_{t})^{T} + (K_{t}H_{t})V(\vec{x}_{t}^{obs}) (K_{t}H_{t})^{T} \\\ &=& V(\vec{x}_{t}^{odom}) - (K_{t}H_{t})V(\vec{x}_{t}^{odom}) - V(\vec{x}_{t}^{odom}) (K_{t}H_{t})^{T} + (K_{t}H_{t})V(\vec{x}_{t}^{odom})(K_{t}H_{t})^{T} + (K_{t}H_{t})V(\vec{x}_{t}^{obs})(K_{t}H_{t})^{T} \\\ &=& (I-K_{t}H_{t})V(\vec{x}_{t}^{odom}) + \left( -V(\vec{x}_{t}^{odom})H^{T} + K_{t}H_{t}V(\vec{x}_{t}^{odom})H_{t}^{T} + K_{t}H_{t}V(\vec{x}_{t}^{obs})H_{t}^{T} \right) K_{t}^{T} \\\ &=& (I-K_{t}H_{t})V(\vec{x}_{t}^{odom}) \ \ \ \ \ \ \ (極致条件(7)により後ろの項は0)\tag{6} \end{eqnarray}$

その他よく出てくる表記法

$x_{a|b}$ は、時刻 $t=t_{b}$ までの観測値を使って推定した時刻 $t=t_{a}$ での推定値のこと。したがって
* $x_{k|k}$ : 時刻 $t=t_{k}$ での推定値のこと。すなわち $x_{t_{k}}^{est}$ のこと。
* $x_{k+1|k}$ : 時刻 $t=t_{k+1}$ での予測値のこと。すなわち $x_{t_{k+1}}^{odom}$ のこと。
* $x_{k+1|k+1}$ : $t=t_{k+1}$ に対して観測結果更新を行った状態、すなわち時刻 $t=t_{k+1}$ での推定値であり $x_{t_{k+1}}^{est}$ のこと。

パラメータ推定（２）カルマンフィルター - Qiita

アルゴリズムまとめ

線形カルマンフィルタは、 $\vec{x}_{t-1}^{est}$ 、 $\vec{x}_{t}^{obs}$ 、 $\vec{u}_{t}$ 、 $V(\vec{x}_{t-1}^{est})$ を入力として $\vec{x}_{t}^{est}$ と $V(\vec{x}_{t}^{est})$ を求めることを目指す。処理の流れを以下に示す。

1: 式(2)により $\vec{x}_{t}^{odom}$ を求める
2: 式(3)により $V(\vec{x}_{t}^{odom})$ を求める
3: 式(7)により $K_{t}^{opt}$ を求める
4: 式(5)により $x_{t}^{est}$ が決まる。
5: 式(6)により $V(\vec{x}_{t}^{est})$ が決まる。

最適カルマンゲインの導出（極値条件を用いた導出）

最適とは推定誤差、すなわち $\vec{e}_{t} \equiv \vec{x}_{t}^{est} - \vec{x}_{t}^{true}$ の分散が最小になることである。共分散行列を $\Sigma (\vec{e}_{t})$ と表記すると

$\begin{eqnarray} \Sigma ( \vec{e}_{t} ) &\equiv& E(\vec{e}_{t} \vec{e}_{t}^{T}) \\\ &=& E[ ( \vec{x}_{t}^{est}-\vec{x}_{t}^{true} ) ( \vec{x}_{t}^{est}-\vec{x}_{t}^{true} )^{T} ] \\\ &=& E[ ( ( I-K_{t}H_{t}\vec{x}_{t}^{odom} )+K_{t}H_{t}\vec{x}_{t}^{obs} - \vec{x}_{t}^{true}) ( ( I-K_{t}H_{t}\vec{x}_{t}^{odom} )+K_{t}H_{t}\vec{x}_{t}^{obs} - \vec{x}_{t}^{true} )^{T} ] \\\ &=& E[\left( (I-K_{t}H_{t}) (\vec{x}_{t}^{odom} - \vec{x}_{t}^{true}) + K_{t}H_{t} ( \vec{x}_{t}^{obs} - \vec{x}_{t}^{true} ) \right) \\\ && \left( ( I-K_{t}H_{t} ) ( \vec{x}_{t}^{odom} - \vec{x}_{t}^{true} ) + K_{t}H_{t} ( \vec{x}_{t}^{obs} - \vec{x}_{t}^{true} ) \right)^{T} ] \\\ &=& E[(\vec{x}_{t}^{odom} - \vec{x}_{t}^{true}) (\vec{x}_{t}^{odom} - \vec{x}_{t}^{true})^{T} ] - 2 ( K_{t}H_{t}) E[ (\vec{x}_{t}^{odom}-\vec{x}_{true})(\vec{x}_{t}^{odom}-\vec{x}_{true})^{T} ] \\\ && + ( K_{t}H_{t} ) E[(\vec{x}_{t}^{odom}-\vec{x}_{t}^{true})(\vec{x}_{t}^{odom}-\vec{x}_{t}^{true})^{T}] (K_{t}H_{t})^{T} \\\ && + ( K_{t}H_{t} ) E[(\vec{x}_{t}^{obs}-\vec{x}_{t}^{true})(\vec{x}_{t}^{odom}-\vec{x}_{t}^{true})^{T}] (K_{t}H_{t})^{T} \\\ &=& V(\vec{x}_{t}^{odom}) -2(K_{t}H_{t})V(\vec{x}_{t}^{odom}) + ( K_{t}H_{t} ) \left( V(\vec{x}_{t}^{odom}) + V(\vec{x}_{t}^{obs}) \right) (K_{t}H_{t})^{T} \end{eqnarray}$

最後の式の展開は $K_{t}$ の次数ごとに項を分解している。ここでは $\vec{x}_{t}$ に関する分散共分散行列を $V(\vec{x}_{t})$ と表記している。 $\vec{e}_{t}$ の分散は共分散行列 $\Sigma (\vec{e}_{t})$ のトレースとして表されるので、分散が最小になるという必要条件から、最適なカルマンゲイン $K_{t}^{opt}$ に関する方程式が以下のように導出できる。

$\begin{eqnarray} 0 &=& \frac{\partial Tr( \Sigma (\vec{e}_{t}))}{\partial K_{t}^{opt}} \\\ &=& -2 V(\vec{x}_{t}^{odom})H_{t}^{T} + 2K_{t}^{opt} \left( H_{t} (V( \vec{x}_{t}^{odom}) + V (\vec{x}_{t}^{obs}) ) H_{t}^{T} \right) \tag{7} \\\ \end{eqnarray}$

ここで、行列のトレースに対する行列微分（スカラーに対する行列微分とは、スカラーを各行列成分で微分したもの）を実施しており、一般に行列 $A$ 、 $B$ に対して

$\begin{equation} \frac{\partial Tr(AB)}{\partial A} = B^{T} \end{equation}$

が成立することを利用している(導出は公式まとめを参照)。これを $K_{t}^{opt}$ について解くと

$\begin{equation} K_{t}^{opt}=V(\vec{x}_{t}^{odom}) H_{t}^{T} \left( H_{t} ( V( \vec{x}_{t}^{odom}) + V ( \vec{x}_{t}^{obs}) ) H_{t}^{T} \right)^{-1} \end{equation}$

また、 $V(\vec{z}_{t}) = H_{t} V(\vec{x}_{t}^{obs})H_{t}^{T} = V(\vec{v})$ なので

$\begin{equation} K_{t}^{opt}=V(\vec{x}_{t}^{odom}) H_{t}^{T} \left( H_{t} V( \vec{x}_{t}^{odom}) H_{t}^{T} + V ( \vec{v}) ) \right)^{-1} \tag{8} \end{equation}$

とも書ける。

最適なカルマンゲインの導出（Likelihoodを使った求め方）

予測による推定結果が $\vec{x}_{t}$ 、観測結果が $\vec{z}_{t}$ であったときに、最終的な推定結果 $\vec{x}_{t}$ となる確率は

$\begin{eqnarray} P(\vec{x}_{t}|\vec{x}_{1:t-1},\vec{z}_{1:t},\vec{v}_{1:t}) &=& \frac{P(\vec{z}_{t}|\vec{x}_{1:t}, \vec{v}_{1:t}, \vec{z}_{1:t-1}) \times P(\vec{x}_{t}| \vec{x}_{1:t-1}, \vec{z}_{1:t-1}, \vec{v}_{1:t})}{P(\vec{z}_{t}|\vec{x}_{1:t-1}, \vec{v}_{1:t}, \vec{z}_{1:t-1})} \ \ \ \ \ (ベイズの定理(1)を利用) \\ &\propto & P(\vec{z}_{t} | \vec{x}_{t}) \times P(\vec{x}_{t} | \vec{x}_{t-1}, \vec{v}_{t}) \ \ \ \ \ (無関係な項を落とした) \\ &\propto& \exp\left[ -\frac{1}{2}(\vec{z}_{t}-H_{t}\vec{x}_{t})^\mathsf{T} V(v)^{-1} (\vec{z}_{t}-H_{t}\vec{x}_{t}) \right] \times \exp \left[ (\vec{x}_{t}-\vec{x}_{t}^{odom})^\mathsf{T}V(\vec{x}_{t}^{odom})^{-1} (\vec{x}_{t}-\vec{x}_{t}^{odom}) \right] \\ &\equiv& \exp(L(t)) \\\ \end{eqnarray}$

ここで、 $\vec{x}_{t}$ の推定値は $L(t)$ を最大にするはずなので、1回微分したものが0にならないといけない。すなわち

$\begin{equation} \vec{0} = \frac{\partial L(t)}{\partial \vec{x}_{t}} = -(\vec{z}_{t} - H_{t}\vec{x}_{t})^\mathsf{T} (V(v)^{-1})^\mathsf{T}(-H_{t}) - (\vec{x}_{t}-\vec{x}_{t}^{odom})^\mathsf{T}(V(\vec{x}_{t}^{odom})^{-1})^\mathsf{T} \\ \end{equation}$

ベクトル微分については公式まとめを参照。扱いやすいように転置をとると

$\begin{eqnarray} H_{t}^\mathsf{T}V(v)^{-1}(\vec{z}_{t}-H_{t}\vec{x}_{t}) &=& V(\vec{x}_{t}^{odom})^{-1}(\vec{x}_{t}-\vec{x}_{t}^{odom}) \\ H_{t}^\mathsf{T}V(v)^{-1}\left[ (\vec{z}_{t}-H_{t}\vec{x}_{t}^{odom}) - (\vec{x}_{t} - \vec{x}_{t}^{odom}) \right] &=& V(\vec{x}_{t}^{odom})^{-1}(\vec{x}_{t}-\vec{x}_{t}^{odom}) \\ H_{t}^\mathsf{T}V(v)^{-1} (\vec{z}_{t}-H_{t}\vec{x}_{t}^{odom}) - H_{t}^\mathsf{T}V(v)^{-1}H_{t}(\vec{x}_{t} - \vec{x}_{t}^{odom}) &=& V(\vec{x}_{t}^{odom})^{-1}(\vec{x}_{t}-\vec{x}_{t}^{odom}) \\ (H_{t}^\mathsf{T}V(v)^{-1}H_{t} + V(\vec{x}_{t}^{odom})^{-1})(\vec{x}_{t}-\vec{x}_{t}^{odom}) &=& H_{t}^\mathsf{T}V(v)^{-1} (\vec{z}_{t}-H_{t}\vec{x}_{t}^{odom}) \\ \vec{x}_{t}-\vec{x}_{t}^{odom} &=& \left[ H_{t}^\mathsf{T}V(v)^{-1}H_{t} + V(\vec{x}_{t}^{odom})^{-1} \right]^{-1} H_{t}^\mathsf{T}V(v)^{-1} (\vec{z}_{t}-H_{t}\vec{x}_{t}^{odom}) \end{eqnarray}$

この式をカルマンゲインの定義式

$\begin{equation} \vec{x}_{t} = \vec{x}_{t}^{odom}+K_{t}(\vec{z}_{t} - H_{t}\vec{x}_{t}^{odom}) \end{equation}$

と見比べると

$\begin{equation} K_{t}^{opt} = \left[ H_{t}^\mathsf{T}V(v)^{-1}H_{t} + V(\vec{x}_{t}^{odom})^{-1} \right]^{-1} H_{t}^\mathsf{T}V(v)^{-1} \end{equation}$

このままでも良いが、「最適カルマンゲインの導出（極値条件を用いた導出）」で求めた表式と一致させるために式変形をしていくと

$\begin{eqnarray} K_{t}^{opt} &=& \left[ H_{t}^\mathsf{T}V(v)^{-1}H_{t} + V(\vec{x}_{t}^{odom})^{-1} \right]^{-1} H_{t}^\mathsf{T}V(v)^{-1} \left[ H_{t}V(\vec{x}_{t}^{odom})H_{t}^\mathsf{T} + V(v)\right] \left[ H_{t}V(\vec{x}_{t}^{odom})H_{t}^\mathsf{T} + V(v)\right]^{-1} \\ &=& \left[ H_{t}^\mathsf{T}V(v)^{-1}H_{t} + V(\vec{x}_{t}^{odom})^{-1} \right]^{-1} \left[ H_{t}^\mathsf{T}V(v)^{-1}H_{t}V(\vec{x}_{t}^{odom})H_{t}^\mathsf{T} + H_{t}^\mathsf{T} \right] \left[ H_{t}V(\vec{x}_{t}^{odom})H_{t}^\mathsf{T} + V(v)\right]^{-1} \\ &=& \left[ H_{t}^\mathsf{T}V(v)^{-1}H_{t} + V(\vec{x}_{t}^{odom})^{-1} \right]^{-1} \left[ H_{t}^\mathsf{T}V(v)^{-1}H_{t} + V(\vec{x}_{t}^{odom})^{-1} \right] V(\vec{x}_{t}^{obs})H_{t}^\mathsf{T} \left[ H_{t}V(\vec{x}_{t}^{odom})H_{t}^\mathsf{T} + V(v)\right]^{-1} \\ &=& V(\vec{x}_{t}^{odom})H_{t}^\mathsf{T} \left[ H_{t}V(\vec{x}_{t}^{odom})H_{t}^\mathsf{T} + V(v)\right]^{-1} \end{eqnarray}$

となり、「最適カルマンゲインの導出（極値条件を用いた導出）」で求めたものと同じ形になることがわかった。ちなみに、推定値の分散 $V(\vec{x}_{t}^{est})$ の逆数は $L(t)$ における $\vec{x}_{t}^\mathsf{T} \vec{x}_{t}$ の係数の-2倍であることから

$\begin{eqnarray} V(\vec{x}_{t}^{est})^{-1} &=& H_{t}^\mathsf{T} V(v)^{-1} H_{t} + V(\vec{x}^{odom})^{-1} \\ V(\vec{x}_{t}^{est}) &=& \left[ H_{t}^\mathsf{T}V(v)^{-1}H_{t} + V(\vec{x}^{odom})^{-1} \right]^{-1} \\ \end{eqnarray}$

と表現される。

1次元カルマンフィルターの例

等速直線運動を考える。

$\begin{equation} x_{t}^{true} = 0.5t+50 + n_{odom} \end{equation}$

ただし $n_{odom}$ は $n_{odom} \sim N ( 0, \sigma _{odom}^{2} )$ のノイズだとすると、カルマンフィルタの定式化に合わせた場合

$\begin{equation} x_{t}^{true} = 1\times x_{t-1}^{true} + 1\times 0.5 + n_{odom} \end{equation}$

となる。また、観測による線形変換を

$\begin{equation} z_{t} = 0.1(x_{t}^{true}+n_{obs}) = 0.1x_{t}^{obs} \end{equation}$

とする。ただし $n_{obs}$ は $n_{obs} \sim N ( 0, \sigma _{obs}^{2} )$ のノイズだとする。この場合 $H_{t}=0.1$ となる。

最適なカルマンゲインは

$\begin{equation} K_{t}^{opt} = \frac{0.1\sigma_{odom}^{2}}{0.1^{2}(\sigma_{obs}^{2} + \sigma_{odom}^{2})} \end{equation}$

となり

$\begin{equation} H_{t}K_{t}^{opt} = \frac{\sigma_{odom}^{2}}{\sigma_{obs}^{2} + \sigma_{odom}^{2}} \end{equation}$

のようにシンプルな形で表されることがわかった。推定量は

$\begin{equation} x_{t}^{est} = \frac{\sigma_{obs}^{2}}{\sigma_{obs}^{2} + \sigma_{odom}^{2}}x_{t}^{odom} + \frac{\sigma_{odom}^{2}}{\sigma_{obs}^{2} + \sigma_{odom}^{2}}x_{t}^{obs} \end{equation}$

と表される。

$\sigma_{obs}=\sigma_{odom}=5.0$ とした場合の結果を以下に示す。右下のグラフから、カルマンフィルタによる推定値（緑）の誤差がオドメトリ（赤）と観測値（青）の誤差より小さくなっており良い推定量になっていることがわかる。

f:id:salpik:20201010233234p:plain — カルマンフィルタの結果

ベイズフィルターの事後確率を分布の重ね合わせとして解釈する

自分の理解としては、線形カルマンフィルターの出力 $P(x_{t} | z_{1:t}, x_{1:t-1}, u_{1:t})$ はガウシアンの掛け合わせになるということである。これは

$\begin{eqnarray} P(x_{t} | z_{1:t}, x_{1:t-1}, u_{1:t}) &=& P(z_{t} | x_{1:t}, z_{1:t}, u_{1:t}) \times P(x_{t} | x_{1:t-1}, z_{1:t-1}, u_{1:t}) \\ &=& P(z_{t} | x_{t}) \times P(x_{t} | x_{t-1}, u_{t}) \\ &=& \frac{P(x_{t} | z_{t}) P(z_{t})}{P(x_{t})} \times P(x_{t} | x_{t-1}, u_{t}) \\ &\propto& P(x_{t} | z_{t}) P(x_{t} | x_{t-1}, u_{t}) \end{eqnarray}$