カルマンフィルタのコンセプト

あるシステムの状態を逐次的（オンライン）に推定する方法で、現実のシステムでのノイズを考慮して、統計的・確率的に確からしい値を算出する方法

利用においては以下の条件がある。

補足：モデリングとは

カルマンフィルタを利用する際には、事前に何かしら対象のシステムを数式的に表現出来ている必要がある。
その数式的に表現することをモデリングというっぽい。
例えば高校物理で学ぶような運動方程式とか。

モデリング方法には運動方程式以外にも種類があり、大きく分けて3つに大別される

#	種類	説明
1	第一原理モデリング（ホワイトボックスモデリング）	運動方程式等。例えば質量など数式上のパラメータも既知
2	システム同定（ブラックボックスモデリング）	内部構造など正確には不明だが、実際の入出力結果からモデリングする方法
3	グレーボックスモデリング	上記２つの中間

対象のシステムが以下のような「線形離散時間状態空間モデル」で記述され、$\boldsymbol{A}$,$\boldsymbol{b}$,$\boldsymbol{c}$が既知（何かしらでモデリングされている）な場合を考える。

\boldsymbol{x}(k+1)=\boldsymbol{Ax}(k)+\boldsymbol{b}v(k)\\
y(k)=\boldsymbol{c}^T\boldsymbol{x}(k)+w(k)

$\boldsymbol{x}(k)$が算出したいシステムの状態
$y(k)$が観測値

上記前提条件に加えて、計算や説明のため下記を定義する

項目	説明	名称
$\boldsymbol{\hat{x}}(k)$	時刻$k$までに利用可能なデータ（$y(k)$も含める）に基づいた、時刻$k$における$\boldsymbol{x}$の推定値	最適推定値や事後推定値という
$\boldsymbol{\hat{x}}^-(k)$	時刻$k-1$までに利用可能なデータに基づいた、時刻$k$における$\boldsymbol{x}$の推定値	予測推定値や事前推定値という
$\boldsymbol{\tilde{x}}(k)=\boldsymbol{x}(k)-\boldsymbol{\hat{x}}(k)$	推定値の真の値の誤差	状態推定誤差という
$\boldsymbol{P}^-(k)=E[\boldsymbol{\tilde{x}}^-(k)(\boldsymbol{\tilde{x}}^-(k)^T)]$	$\boldsymbol{\tilde{x}}^-(k)$の共分散行列
$\boldsymbol{P}(k)=E[\boldsymbol{\tilde{x}}(k)(\boldsymbol{\tilde{x}}(k)^T)]$	$\boldsymbol{\tilde{x}}(k)$の共分散行列

※若干複雑なので省略、詳しくは参考書籍などを読んでください。といっても高校数学ぐらいの知識で理解可能だと思います。

名称	式
事前状態推定値	$\boldsymbol{\hat{x}^-}(k)=\boldsymbol{A}\boldsymbol{\hat{x}}(k-1)$
事前誤差共分散行列	$\boldsymbol{P}^-(k)=\boldsymbol{A}\boldsymbol{P}(k-1)\boldsymbol{A}^T+\sigma^2_v\boldsymbol{b}\boldsymbol{b}^T$

名称	式
カルマンゲイン	$\boldsymbol{g}(k)=\frac{\boldsymbol{P}^-(k)\boldsymbol{c}}{\boldsymbol{c}^T\boldsymbol{P}^-(k)\boldsymbol{c}+\sigma^2_w}$
状態推定値	$\boldsymbol{\hat{x}}(k)=\boldsymbol{\hat{x}}^-(k)+\boldsymbol{g}(k)(y(k)-\boldsymbol{c}^T\boldsymbol{\hat{x}}^-(k))$
事後誤差共分散行列	$\boldsymbol{P}(k)=(\boldsymbol{I}-\boldsymbol{g}(k)\boldsymbol{c}^T)\boldsymbol{P}^-(k)$

「予測ステップ(k)」は「フィルタリングステップ(k-1)」から算出でき、
「フィルタリングステップ(k)」は「予測ステップ(k)」と観測値$y(k)$から算出できる。

初期値設定
- 初期値k=0で、$\boldsymbol{\hat{x}}(0)=\boldsymbol{x_0}$、$\boldsymbol{P}(0)=\sum_0$とおく
- $\sigma^2_v$と$\sigma^2_w$も何らかの値に設定する
予測ステップ(k=1)の算出
フィルタリングステップ(k=1)の算出
予測ステップ(k=2)の算出
フィルタリングステップ(k=2)の算出
予測ステップ(k=3)の算出

と、延々繰り返す。
これがカルマンフィルタの大きなながれ。

カルマンフィルタを利用するには対象のシステムを

「線形離散時間状態空間モデル」

\boldsymbol{x}(k+1)=\boldsymbol{Ax}(k)+\boldsymbol{b}v(k)\\
y(k)=\boldsymbol{c}^T\boldsymbol{x}(k)+w(k)

の形で表現する必要がある。
そのため運動方程式など別途モデリングしていた数式をこの形に変形する必要があります。
具体的な式変形などはここでは省きます。

上記は線形なシステムモデルにしか利用出来ないが、非線形なシステムモデルへの利用に対応するため、
EKF（拡張カルマンフィルタ）や UKF (Unscented Kalman Filter)という手法もある。

他にも制御入力を含んだ形のカルマンフィルタなどもある。

$\sigma^2_v$と$\sigma^2_w$の設定方法は、これ、という方法はなさそう？
（そういう研究もありそうですが調べてないです）
各システム毎にそれらしい値を調整しながら適用するのが基本的な利用方法になるようです。

正しく理解出来ているか不安な中、参考として載せさせていただくのも恐縮ですが書かないのも問題だと思うので記載させていただきます。