satomacoto

June 22, 2011

Pythonで拡張カルマンフィルタを実装してみる

拡張カルマンフィルタ(EKF; Extended Kalman Filter)は非線形カルマンフィルタのひとつ。線形カルマンフィルタは線形システムを対象としていましたが、拡張カルマンフィルタは非線形システムを対象とします。ナビゲーションやらGPSやらに利用されている、らしい。

システムが非線形のとき、すなわち

${\bf x}_{t+1}=f({\bf x}_{t},{\bf u}_{t})+{\bf w}_{t+1}$ （状態方程式）
${\bf y}_{t}=h({\bf x}_{t})+{\bf v}_{t}$ （観測方程式）
${\bf w}_{t}\sim N(0,{\bf Q})$ ${\bf v}_{t}\sim N(0,{\bf R})$ （ノイズは正規分布）
$p({\bf x}_{t}|{\bf y}_{0:t})=N({\bf x}_{t};{\bf \mu}_{t},{\bf \Sigma}_{t})$ （状態は正規分布）

とするとき、関数 f は前の状態から推定値を与え、関数 h は観測値を与えますが、どちらの関数も直接共分散を求めることはできません。が、拡張カルマンフィルタでは状態方程式も観測方程式も微分可能であれば線形である必要はありません。

拡張カルマンフィルタでは状態方程式と観測方程式の線形化をするために、線形カルマンフィルタにおける時間遷移モデルと観測モデルに各関数の偏微分行列（ヤコビアン）を用います。

${\bf A}_{t} = \left\ \frac{\partial f({\bf x}_{t},{\bf u}_{t})}{\partial {\bf x}_{t}} \right|_{{\bf \mu}_t, {\bf u}_t}$
${\bf C}_{t} = \left\ \frac{\partial h({\bf x}_{t})}{\partial {\bf x}_{t}} \right|_{{\bf \mu}_t}$

あとは、線形カルマンフィルタと同じ。 μ_t, Σ_t, u_t, y_t+1 を入力として、 μ_t+1, Σ_t+1を出力します。1ステップのプロセスは以下のとおり。

# prediction

${\bf \hat{\mu}}_{t+1} \leftarrow f({\bf \mu}_{t},{\bf u})$ （現在の推定値）
${\bf A}_{t} \leftarrow \left\ \frac{\partial f({\bf x}_{t},{\bf u}_{t})}{\partial {\bf x}_{t}} \right|_{{\bf \mu}_t, {\bf u}_t}$ （状態方程式の現在のヤコビアン）
${\bf \hat{\Sigma}}_{t+1} \leftarrow {\bf Q}+{\bf A}_t{\Sigma}_t {\bf A}_t^T$ （現在の誤差行列）

# update

${\bf C}_{t+1} \leftarrow \left\ \frac{\partial h({\bf x}_{t})}{\partial {\bf x}_{t}} \right|_{{\bf \hat{\mu}}_{t+1}}$ （観測方程式のヤコビアン）
${\bf \tilde{y}}_{t+1} \leftarrow {\bf y}_{t+1}-{\bf C}_{t+1}{\hat{\mu}}_{t+1}$ （観測残差）
${\bf S}_{t+1} \leftarrow {\bf C}_{t+1}{\bf \hat{\Sigma}}_{t+1}{\bf C}_{t+1}^T+{\bf R}$ （観測残差の共分散）
${\bf K}_{t+1} \leftarrow {\bf \hat{\Sigma}}_{t+1}{\bf C}^T{\bf S}^{-1}$ （最適カルマンゲイン）
${\bf \mu}_{t+1} \leftarrow {\bf \hat{\mu}}_{t+1}+{\bf K}_{t+1}{\bf \tilde{y}}_{t+1}$ （更新された現在の推定値）
${\bf \Sigma}_{t+1} \leftarrow {\bf \hat{\Sigma}}_{t+1}-{\bf K}_{t+1}{\bf C}_{t+1}{\bf \hat{\Sigma}}_{t+1}$ （更新された現在の誤差行列）

例題。

2次元座標において、あるロボットがt=0に原点を出発して、速度(2,2)で動くとする。ロボットの進路は風などの影響を受け（σ_x=σ_y=1）、毎秒ごと3つの点(0,0),(10,0),(0,10)からの距離を計測できて、計測には距離によらない誤差がある（σ_x=σ_y=2）とする。このとき、観測された軌跡から実際の軌跡を推定する。

Fig.1 ロボットの位置は(x₀,x₁)で、3点からの観測値(y₀,y₁,y₂)を得る。

状態方程式は線形、観測方程式は非線形となります。

$f({\bf x},{\bf u})=\left[\begin{a}{cc}1&0\\0&1\end{}\right]{\bf x}+\left[\begin{a}{cc}1&0\\0&1\end{}\right]{\bf u},\ \ \ {\bf u}=\left[\begin{a}{c}2\\2\end{}\right]$
$h({\bf x})=\left[\begin{array}{c} \sqrt{x_0^2+x_1^2}\\ \sqrt{(x_0-10)^2+x_1^2}\\ \sqrt{x_0^2+(x_1-10)^2} \end{array}\right]$

観測方程式のヤコビアンは

$J=\left[\begin{array}{cc} \fr{\pa{h}_0}{\pa{x_0}}&\fr{\pa{h}_0}{\pa{x_1}} \\ \fr{\pa{h}_1}{\pa{x_0}}&\fr{\pa{h}_1}{\pa{x_1}} \\ \fr{\pa{h}_2}{\pa{x_0}}&\fr{\pa{h}_2}{\pa{x_1}} \\ \end{array}\right]$

として求められます。実装では上式を解析的に解く場合、数値的に解く場合を試してみます。なおSciPyには数値的にヤコビアンを求める関数が用意されているので、ここではそれを使ってみます。（微小区間ズラして傾き求めている…）

以下、実装。61行目と62行目で解析的か数値的か切り替えます。

割とうまくいっている実行結果。

Fig. 2 赤が実際の軌跡。青が推定値。
左が解析的なヤコビアン、右が数値的なヤコビアンを使った結果。

なんかうまくいってないような(?)結果も出てしまいます。

Fig. 3 なんか微妙…

UKFも実装してみようかな。

http://docs.scipy.org/doc/scipy/reference/optimize.html
http://old.nabble.com/calculating-numerical-jacobian-td20506078.html
http://projects.scipy.org/scipy/browser/trunk/scipy/optimize/slsqp.py
http://en.wikipedia.org/wiki/Broyden's_method

June 19, 2011

Pythonでカルマンフィルタを実装してみる

カルマンフィルタは、時間変化するシステムの、誤差のある離散的な観測から現在の状態を推定する手法。Wikipediaの記事（カルマンフィルター）がわかりやすい。

状態方程式と観測方程式が次のように与えられているとき

${\bf x}_{t+1}={\bf Ax}_{t}+{\bf Bu}_{t}+{\bf w}_{t+1}$ （状態方程式）
${\bf y}_{t}={\bf Cx}_{t}+{\bf v}_{t}$ （観測方程式）
${\bf w}_{t}\sim N(0,{\bf Q})$ ${\bf v}_{t}\sim N(0,{\bf R})$ （ノイズ）
$p({\bf x}_{t}|{\bf y}_{0:t})=N({\bf x}_{t};{\bf \mu}_{t},{\bf \Sigma}_{t})$ （フィルタ分布）

線形カルマンフィルタ(LKF; Linear Kalman Filter)は μ_t, Σ_t, u_t, y_t+1 を入力として、 μ_t+1, Σ_t+1を出力する。1ステップのプロセスは以下のとおり。

# prediction

${\bf \hat{\mu}}_{t+1} \leftarrow {\bf A\mu}_{t}+{\bf Bu}$ （現在の推定値）
${\bf \hat{\Sigma}}_{t+1} \leftarrow {\bf Q}+{\bf A\Sigma}_t {\bf A}^T$ （現在の誤差行列）

# update

${\bf \tilde{y}}_{t+1} \leftarrow {\bf y}_{t+1}-{\bf C\hat{\mu}}_{t+1}$ （観測残差）
${\bf S}_{t+1} \leftarrow {\bf C}{\bf \hat{\Sigma}}_{t+1}{\bf C}^T+{\bf R}$ （観測残差の共分散）
${\bf K}_{t+1} \leftarrow {\bf \hat{\Sigma}}_{t+1}{\bf C}^T{\bf S}^{-1}$ （最適カルマンゲイン）
${\bf \mu}_{t+1} \leftarrow {\bf \hat{\mu}}_{t+1}+{\bf K}_{t+1}{\bf \tilde{y}}_{t+1}$ （更新された現在の推定値）
${\bf \Sigma}_{t+1} \leftarrow {\bf \hat{\Sigma}}_{t+1}-{\bf K}_{t+1}{\bf C}{\bf \hat{\Sigma}}_{t+1}$ （更新された現在の誤差行列）

観測を得るごとにPredictionとUpdateを繰り返すことで、現在の状態を推定します。

導出は後述（予定）。

例題を。

2次元座標において、あるロボットがt=0に原点を出発して、速度(2,2)で動くとする。ロボットの進路は風などの影響を受け（σ_x=σ_y=1）、毎秒ごとに観測できるGPSによる位置座標には計測誤差（σ_x=σ_y=2）があるとする。このとき、観測された軌跡から実際の軌跡を推定する。

係数は

${\bf A}=\left[ \begin{array}{cc} 1 & 0 \\ 0 & 1 \\ \end{array} \right]$ ${\bf B}=\left[ \begin{array}{cc} 1 & 0 \\ 0 & 1 \\ \end{array} \right]$ ${\bf u}_{const}=\left[ \begin{array}{c} 2 \\ 2 \\ \end{array} \right]$ ${\bf Q}=\left[ \begin{array}{cc} 1 & 0 \\ 0 & 1 \\ \end{array} \right]$
${\bf C}=\left[ \begin{array}{cc} 1 & 0 \\ 0 & 1 \\ \end{array} \right]$ ${\bf R}=\left[ \begin{array}{cc} 2 & 0 \\ 0 & 2 \\ \end{array} \right]$

初期値は

${\bf \mu}_0=\left[ \begin{array}{c} 0 \\ 0 \\ \end{array} \right]$ ${\bf \Sigma}_0=\left[ \begin{array}{cc} 0 & 0 \\ 0 & 0 \\ \end{array} \right]$

コードは以下。要SciPy。（やってる事自体はSciPyは必須ではないのだけど。）

実行結果は下図。

Fig.1 ■が実際の軌跡。▲が観測値。●が推定値。

観測に対してカルマンフィルタによる推定値が、実際の軌跡に近いことがわかります。

Python+SciPyでk-meansを実装してみる

k-means（K平均法）はクラスタリング手法の一つ。

PythonではSciPyに実装されているので簡単に利用することができます。
Macではこちらの記事に書いてある方法で関連するソフトもインストールできます。

たとえば下図のように2種類の2次元正規分布からサンプリングされたデータがあるとき

k-meansを用いると下図のようにクラスタリングされます。

以下、コード。

SciPyを用いるとk-meansは21~22行目の実質2行で利用することができます。

なおサンプリングについてはここを、kmeansについてはここを参照。

ちなみに正規化しないと下図のようになってしまいます。

k-meansのアルゴリズムについては、鹿島先生の講義資料の概論②の14~16ページの説明なんかが分かりやすいかもしれません。

May 22, 2011

HomebrewでMySQL 5.5.10をインストール

HomebrewでMySQLをインストール。

brew install mysql

でインストール。この後にすべきことはすべてターミナルに表示されています。ただ、ちゃんと見ないで適当に進めていたら、動きませんでした。ターミナルは管理者権限で実行しなければならないものもあります。（コマンドの前に sudo を付ける）

unset TMPDIR
mysql_install_db --verbose --user=satomacoto --basedir="$(brew --prefix mysql)" --datadir=/usr/local/var/mysql --tmpdir=/tmp

satomacotoはオーナー権限のある自分のユーザ名を。--verboseで色々出力されているはずなので、この時点でエラーが起きていないか要確認。

（追記）
5.5.14にするときはwhoamiを使った

mysql_install_db --verbose --user=`whoami` --basedir="$(brew --prefix mysql)" --datadir=/usr/local/var/mysql --tmpdir=/tmp

（追記終わり）

mysql.server start

で起動。なお、停止は mysql.server stop

/usr/local/Cellar/mysql/5.5.10/bin/mysql_secure_installation

でパスワードなど設定。

mysql -uroot -p

でrootで接続。

mkdir -p ~/Library/LaunchAgents
cp /usr/local/Cellar/mysql/5.5.10/com.mysql.mysqld.plist ~/Library/LaunchAgents/
launchctl load -w ~/Library/LaunchAgents/com.mysql.mysqld.plist

でOS起動時にMySQL起動。なお、このままだと、mysql.server stop で停止しようとしてもすぐに再起動されてしまうので、停止するときは launchctl unload -w ~/Library/LaunchAgents/com.mysql.mysqld.plist する。phpAdminがうまく動かないものだから、Property List EditorでWorkingDirectoryとUserNameの項目は削除してしまった。

ソケットはデフォルトで /tmp/mysql.sock 。以下で確認。

mysql_config --socket

うまく行かなかったので何度かやり直した。
削除は、

brew uninstall mysql
sudo rm -rf /usr/local/var/mysql

MySQL 5.5ではデフォルトでutf-8設定になっている。

May 5, 2011

Googleの検索結果に言語を変更するためのリンクを追加するChrome Extension

Googleの検索設定では、たとえば表示言語を日本語に設定していると、英語だけの検索結果がほしいとき、優先する検索言語を英語“のみ”にすることが出来ません。たぶん。そこで、検索言語を変更するためのリンクをGoogleのサイドバーに追加するChromeの拡張機能を作ってみました。

インストールは以下のページから。
Language Links for Google - Chrome Web Store

（以前にも、Greasemonkeyで似たようなものを幾つか作りました。今は動くかわかりません。）

Google Language Links サイドバーにリンクを追加。
Google Search Language Jumper 演算子っぽく。（解説）
Google Search Language Selector 検索窓の下にラジオボタンを追加。（解説）

使い方。

インストールしてGoogleで検索するとアドレスバーにアイコンが表示されます。これをクリックするとオプションページに飛びます。

オプションページで追加する言語を選択し保存します。

サイドバーに選択した言語での検索結果へのリンクが追加されます。

リンクをクリックすると該当する言語で検索を行います。
（まだうまく動かない時もあります…。）

以下、開発メモ。

Instantへの対応

function addLink() {
...
}

document.addEventListener('DOMNodeInserted', addLink, false);

ページ要素が変化するたびにリンク先を変更。割と力技。もっと効率のよいやり方があるような。

Page actionの表示

background.html

chrome.extension.onRequest.addListener(function(request, sender, sendResponse) {
    if (request.method == "getLang") {
        sendResponse({lang: JSON.parse(localStorage.getItem("lang"))});
        chrome.pageAction.show(sender.tab.id);
    }

Page Actions - Google Chrome Extensions - Google CodeのExamplesから…

Content scriptでlocalstorageを使用

Chrome extension accessing localstorage in content script - Stack Overflow

複数言語への対応

国際化 Chrome Extensions API リファレンス