カルマンフィルタの歴史ももう長いことと思いますが，各所で各人なりにまとめられているあたり，なかなか理解の難しいものなんだなあと感じています．
例に漏れず自分もカルマンフィルタの原理がいまいちピンとこなかったので自分なりにまとめてみました．
カルマンフィルタ(Kalman Filter)はフィルタと名付けられてはいますが，使われ方としてはフィルタよりも現代制御のオブザーバ（Observer; 状態推定器）に近いような気がします．
観測した情報から，状態ベクトルが真値に最も近くなるように推定するものです．
概念は以下のところでわかりやすく説明されています．
シンプルなモデルとイラストでカルマンフィルタを直感的に理解してみる
ただ，数式として少し省略されているところもあったので，改めてまとめるに至りました．
基本的に，代数とベイズの定理のみから丁寧に導出していこうという感じです．
カルマンゲインの式の導出，予測値の算出，分散の更新は天下り的な部分も多いので，今回はこれらの導出に焦点を当ててみました．
統計的にみて，予測値と実プラントの誤差が最小となるように演繹的に求めていくとカルマンフィルタの式に行き着くあたり，統計学の応用先の代表例になっているだけのことはあるなあ，という感想です．
ここでは，状態空間と観測空間が同一の場合のみ扱います．
カルマンフィルタは，基本的に離散時間系で利用されることが多いと思いますので，
ここでは連続時間系については言及しません．（単純に理解していないというのもある）
状態空間と観測空間が同一の場合には，状態方程式は以下の式で表されます．
現実世界のプラントの挙動は，状態に対するノイズと観測に対するノイズを含めると次のように書き換えられます．ここで，ノイズはガウス性（正規分布）を仮定します．
仮定に置いたとおり，（線形）カルマンフィルタはノイズがガウス性でない場合は考慮されておらず，実際に使用してみても真値と推定値が合わないことが多いです．
このような場合はパーティクルフィルタ（Particle Filter; 粒子フィルタ）や，アンサンブルカルマンフィルタ(Ensemble Kalman Filter)がよく合うように思います．
このような系で，観測値から状態を得たい場合にはどうするでしょうか．
まずひとつが，オブザーバと似たような考え方で，運動モデルを立てる方法です．
- 運動モデルを立てる．
- 運動モデルに実プラントに与えた入力と同じ入力を与える．
- 運動モデルの出力を状態の推定値として利用する．
この方法では，きちんとモデル化できていて初期値も合っているようであればよいのですが，現実にはノイズが入ってくるため時間を追うごとに誤差が蓄積していきます．
一方で上手くモデル化できていれば，短い時間の範囲では真値と推定値はよく合います．
もうひとつが，観測値から逆算する方法です．
かなり強引に言うと，状態の推定値とする方法です．
ただし，は観測ノイズを含みますので，実際には，
となり，観測ノイズを拾ってしまいます．
しかし，動的な特性のない，長い時間一定の値が連続するような条件であれば真値と推定値はそれなりに合います．
極端な話では，一定の値にノイズが乗っているような場合には長い時間の平均を取れば，推定値は真値に近くなります．
テスタで電池の電圧を測るとき，繋いだ瞬間の急激な変化は表示の遅れなどが出て取りづらいですが，数秒経つと表示が安定して正しく測定できるのに少し似ているかもしれません．
このような，急峻な変化に適した測定と，緩やかな変化に適した測定を上手く組み合わせるのがカルマンフィルタというわけです．
カルマンフィルタによって，動特性の異なる複数のセンサから真値を推定するセンサフュージョンなるものがあります．
構造的にカルマンフィルタはセンサフュージョンに適しているのではないでしょうか．
さて，これをどのように組み合わせればよいのか，という問題です．
単純に両者の平均を取った場合を考えます．
この場合，モデルによって推定された状態も観測値から推定された状態も等しい重みとなります．
いま，モデルがかなり正確な一方，観測値では大きくノイズを拾ってしまうようなとき，両者の重み（重要度）を等しくするのはあまり懸命でないような感じがします．
感覚的には，モデルによって推定された状態の重みを大きく，観測値から推定された状態の重みを小さくとるのではないでしょうか．
係数の合計がであるとすると，重み係数を利用して以下のように書き換えられます．
このを最適となるように決めることができれば，得られる情報から尤もらしさの高い状態が推定できそうです．
これが，カルマンフィルタの基本的な考え方です．
具体的に尤もらしいとはどのようなことか，について考えます．
正規分布なノイズを仮定する場合，尤もらしさの高い状態とは，分散が小さい状態と言い換えることができます．
いま，推定値と真値の差を取ってみます．いわゆる偏差（誤差）です．
両辺の分散は以下となります．
ここではの分散を表します．
分散は，変数が無相関の場合には線形性が成り立つので右辺は，
となります．（たしか分散は絶対値を取ったはず……）
また，定数を含む場合のより，
が成り立ちます．
この左辺の分散が最小となるような重み係数を求めたいので，両辺をで微分し左辺をとします．
でまとめると，
について解くと，
となり，最適な重み係数が求まります．
これらの分散を記号を使って書き直したものがよく見る以下のカルマンゲインの式です．
カルマンゲイン
これでカルマンゲインが求まったので，それらのパラメータである分散とを求めます．
分散はだったので，
であるので，
を代入して，
ここで，より，となり，
これでパラメータが求められました．
ここで，は操作量（入力）の分散で，設計すべきパラメータです．
操作量に関する分散が既知であればそれを使えばいいですし，未知なら適当な値を入れてしまいます．
をと書く場合もあるようで，その場合には次のように書き換えられます．
このパラメータ（分散）は，ある初期値から時々刻々と更新されていきます．
プログラムなんかで実装する場合にはグローバルもしくはstatic変数でこの値を保持する必要があります．
分散は観測の分散で，これも設計パラメータです．
ここにはセンサの分散などを入れればよいです．たぶん．
これを時不変として，と扱うことが多いように思います．
大抵の場合，設計パラメータとは試行錯誤的に決定することが多いのではないかと思います．
これで，カルマンゲインのパラメータが全て求まりました．
カルマンゲインについてまとめると，以下のようになります．
さて，状態の推定ですが，以下の式を少し変形します．
ここで，，およびだったので，
ここに先ほど求めたを代入することで，現在の時刻における尤もらしい状態の推定値を得ることができました．
次にカルマンフィルタの肝要な部分のもうひとつである，分散の更新ついて考えます．
カルマンゲインを計算するために，との2つの分散を使用しました．
は設計パラメータとして与えるべきパラメータで，もう一方のは以下の式で表現されました．
このとき，も設計パラメータで，はプラントモデルの定数でした．
上の式をよく見ると，次の時刻での分散は，初期値のみによって決まってしまうことがわかるかと思います．
せっかくモデルを利用した予測値と観測値からカルマンゲインを計算して，
尤もらしさの高い，言い換えると分散の小さいはずの推定値を手に入れたにも関わらず，
それが反映されていないため次の時刻の分散はどんどん大きくなってしまうのです．
これは単純に，右辺第2項で時刻ごとにを足し続けることからも想像できるかと思います．
このため，分散が小さくなっているはずの推定値を利用して，次の時刻での分散を更新します．
最終的に求められた推定値は，以下の通りでした．
ここで，カルマンゲインで括ったの式を使うと，で括られていないため各項の無相関が成り立ちません．
このため，面倒なのでで括られていない方の式を使っています．
この分散を取ると，
カルマンゲインの2乗項で括ると，
ここで，と変形できるのでこれを代入すると，
となり，カルマンゲイン導出後の予測値の分散を求めることができました．
この分散（事後分布）を次のステップで使用することで，現在のステップよりも分散が小さい条件で推定を行うことが出来るようになります．
上の分散の式にを代入すると，
となり，数式から見ても次ステップの分散は現在の分散よりも小さくなることがわかります．
このあたりのことは参考にさせていただいたページに詳しく記載されています．
このような感じで，天下りのない仮定の下でカルマンフィルタの式を導出することができました．
今回は対象が1次元で，状態空間と観測空間が同一の場合のみについて言及しましたが，
対象が多次元（ベクトルと行列）で，状態空間と観測空間が異なる場合にも同じアプローチで導出できます．
対象が多次元の場合にはシステム全体が行列表記になりますが，基本的には以下の部分を抑えておけば1次元の場合と考え方は殆ど変わりません．
- 太字で書かれているのはベクトルか行列，細字はスカラ
- 行列の積は左右で交換できない
- ベクトル，行列の2乗はのように転置して掛ける
- 1次元での割り算は多次元では逆行列になる
- 数値の1は多くの場合，単位行列になる
状態空間と観測空間が異なる場合には観測方程式に新しくが入って，
であるので，予測の式が以下のように書き換わります．
これはつまり，
であり，カルマンゲインが新たにに変わっただけと見ることができます．
状態を予測するために，状態同士で演算する必要があるため，右辺第２項の係数に右辺第１項の係数を合わせた形です．
をに置き換えると（全く必要ないが），
となり，これはカルマンフィルタの式そのものです．
このような感じで，式を演繹的に追っていくことで，なんとなくカルマンフィルタでやろうとしていることが理解できた気がします．
再掲しますが，以下のページが理解の大きな助けとなりました．
0 件のコメント:
コメントを投稿