はじめに断っておきます。制御工学は素人です。
(大学時に教養としてカジッた程度)
趣味でドローン（マルチコプター）について調べていて、カルマンフィルタが出てきたので学んで見てます。
誤ったところもあるかと思いますので、タイトルも保険を掛けたような感じなってますのであしからず。
カルマンフィルタのコンセプト
あるシステムの状態を逐次的（オンライン）に推定する方法で、現実のシステムでのノイズを考慮して、統計的・確率的に確からしい値を算出する方法
利用においては以下の条件がある。
- そのシステムは何かしらの方法によってモデリングされている必要がある
- システムの状態を適宜観測できる必要がある
補足：モデリングとは
カルマンフィルタを利用する際には、事前に何かしら対象のシステムを数式的に表現出来ている必要がある。
その数式的に表現することをモデリングというっぽい。
例えば高校物理で学ぶような運動方程式とか。
モデリング方法には運動方程式以外にも種類があり、大きく分けて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)
- $k$：離散的（連続的ではない）な時間を示す
- $\boldsymbol{x}(k)$：$n$次元状態ベクトル
- $y(k)$：スカラ時系列データ
- $\boldsymbol{A}$：$n \times n$行列
- $\boldsymbol{b}$：n次元列ベクトル
- $\boldsymbol{c}$：n次元列ベクトル
- $v(k)$：平均値0,分散 $\sigma_v^2$の正規正白色雑音
- $w(k)$：平均値0,分散 $\sigma_w^2$の正規正白色雑音
$\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)$の共分散行列 |
すると、なんだかんだと以下の式が導き出される(※)
※若干複雑なので省略、詳しくは参考書籍などを読んでください。といっても高校数学ぐらいの知識で理解可能だと思います。
予測ステップ(時刻=k)の式2つ
| 名称 | 式 |
|---|---|
| 事前状態推定値 | $\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$ |
フィルタリングステップ(時刻=k)の式3つ
| 名称 | 式 |
|---|---|
| カルマンゲイン | $\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)の算出
と、延々繰り返す。
これがカルマンフィルタの大きなながれ。
補足1
カルマンフィルタを利用するには対象のシステムを
「線形離散時間状態空間モデル」
\boldsymbol{x}(k+1)=\boldsymbol{Ax}(k)+\boldsymbol{b}v(k)\\
y(k)=\boldsymbol{c}^T\boldsymbol{x}(k)+w(k)
の形で表現する必要がある。
そのため運動方程式など別途モデリングしていた数式をこの形に変形する必要があります。
具体的な式変形などはここでは省きます。
補足2
上記は線形なシステムモデルにしか利用出来ないが、非線形なシステムモデルへの利用に対応するため、
EKF（拡張カルマンフィルタ）や UKF (Unscented Kalman Filter)という手法もある。
補足3
他にも制御入力を含んだ形のカルマンフィルタなどもある。
補足4
$\sigma^2_v$と$\sigma^2_w$の設定方法は、これ、という方法はなさそう？
（そういう研究もありそうですが調べてないです）
各システム毎にそれらしい値を調整しながら適用するのが基本的な利用方法になるようです。
参考
正しく理解出来ているか不安な中、参考として載せさせていただくのも恐縮ですが書かないのも問題だと思うので記載させていただきます。