R で Unscented Kalman Filter (UKF)
こんなのに二日もかけて効率悪すぎた・・・
最近、RでUnscented Kalman Filter (UKF) のシミュレーションプログラムを書いていて、全然うまくいかなったんですけど、やっとこさ上手くいって嬉しかったのでブログに書きます。いきなりカルマンフィルターでしかもUnscentedってなんだよって感じですが、非線形で正規性を持つ状態空間モデルにカルマンフィルターを適用しようじゃないかということで考案されたものです。他に有名なものはExtended Kalman Filter (EKF) や Ensenble Kalman Filter (EnKF) があります。EKFはカルマンフィルターさえ理解すれば簡単にわかると思います(ざっくり言うと、非線形関数のテイラー展開を利用して線形近似してカルマンフィルターを適用するだけです)。EnKFはこれから勉強します。
一方UKFですが、これEKFとはかなり違うアイデアに基づいていて、ぶっちゃけカルマンフィルタという名はついているもののその面影はあまりないような気もします。
さて、ここで考える状態空間モデルは次のような式で表されるとします。
ここで はそれぞれ非線形な関数としましょう。実際モデルを考える上でははを次元ベクトルとした場合にp次の単位ベクトルとすることがほとんどだと思います。(そういうものしかみたことがありません。。。もしそうでなかったらごめんなさい)このようなモデルを考えた時のフィルターを考えたい。つまり、「一期前あるいはその時点までの観測値が得られた下での状態の条件付き分布の平均と分散共分散行列の推定」に興味があるんだよ!というのがモチベーションになります。フィルターを考える上でこれを忘れたり、心に止めておかないと何やってんだっけとなることが多いので個人的には大切なポイントだと思ってます。
Unscented Transformation (U変換)
U変換については、その本質的な意味合いをまだ消化しきれていないので、詳細は他の文献にお任せします(これから理解深めなきゃ・・・) 。参考文献からそっくりそのままもってくると、「正規分布に従う確率変数が非線形な変換によって写された先の分布が正規分布でないような場合を仮定し、元の分布、つまり正規分布からサンプリングした点を非線形変換して得られたサンプルを用いて平均と分散を近似する」という試みです。
U変換の手順は次のようなものになります。
として、 となるような非線形な関数 を考えます。このとき は平均0ベクトル、共分散行列を持つp変量正規分布に従うとします。このとき、シグマポイントと呼ばれるサンプルを2m+1個要素として持つ集合 とその重み を次の条件を満足するように取ってきます。
このとき、 の分布の平均と分散を次のように近似することができます。 として、
このときシグマポイントは
ここで の は を満たすように取ると良いでしょうとのことです。また は の平方根行列のi列目のベクトルとします。(ここではコレスキー分解による平方根行列とします。)
最後にU変換をする関数を書いてみたのでそれを載っけて、近似がなかなか上手くいってることを示して終わりにしようと思います。
gistd25a7fa764618e82a814d3c7c35502f6
まずは2変量正規分布から乱数を発生させて、次の非線形な関数によって正規分布に従わないような確率ベクトルを生成します。
このときのデータ点を黒で、平均と分散共分散行列に相当する楕円を青でそれぞれ描いて、赤い線でU変換による平均と分散共分散行列の近似を重ね書きしてみます。
この関数の場合、かなりの精度で近似できてます。この図のスクリプトは以下。
gistfdb330290067dcf0cb63e3a8e391cb2d
上手くいかない例なども気になるので今後の課題です。次回はU変換によるフィルターの構成とその近似精度がどれくらいなのか図示するところまで書きたいなぁと思っています。
[参考文献] 「カルマンフィルタの基礎」 足立修一・丸太一郎 共著