« バッテリの持ち具合 | トップページ | チューニング、チューニング、チューニング »

2009年11月 9日 (月)

カルマンフィルタに不連続なセンサデータを与えてはいけない

11/7(土)の試走会参加を見送り、11/8(日)のTANTOスキークラブの秋のパーティも欠席して、ロボットの自己位置推定のためのカルマンフィルタチューニングに励んでおります。

チューニングの対象はプロセスノイズの共分散なわけですが、チューニングの前にカルマンフィルタのモデルを正しく作れているか、計測ノイズの共分散が妥当な値になっているか、カルマンフィルタに与えるセンサデータを(生のセンサデータから)正しく生成できているか確認するのに手間取りました。

なかでも一番苦戦したのがロボットの進行方向です。デジタルコンパスの値は0~360度(磁北を0度とする)なのですが、最初はロボットが磁北を向いたときに359.9度と0.0度の間で値が不連続になることに気付かず、カルマンフィルタの出力がでたらめになってしまいました。よく考えてみれば、カルマンフィルタは状態が連続的に変化することを前提としているので、値が突然360度も飛んでしまえばイノベーション(計測値に対する観測と予測の差)が大きくなってしまい、計測値の尤もらしさが急激に損なわれます。

それではこまるので、こんな感じで値の範囲を -2nπ~2nπに拡張してみました。

            // a は東向きを 0 とする。(コンパスの出力は磁北が 0[deg])
            // a の範囲を 0~360 [deg] から -PI~PI [rad] にする。
            double temp_deg_a = currentRawData.compass + 90.0;
            if (temp_deg_a > 180)
                temp_deg_a -= 360;
            else if (temp_deg_a < -180)
                temp_deg_a += 360;

            data.a = GeoProjection.deg2rad(temp_deg_a);

            // a が不連続になるとカルマンフィルタに使用できないので、
            // a の範囲を -2nPI ~ 2nPI に拡張する。
            double last_a_deg = lastRawData.compass + 90.0;
            if (last_a_deg > 180)
                last_a_deg -= 360;
            else if (last_a_deg < -180)
                last_a_deg += 360;

            double last_a = GeoProjection.deg2rad(last_a_deg);

            if ((last_a - data.a) > (Math.PI * 1.8))
                n++;
            else if ((last_a - data.a) < (-Math.PI * 1.8))
                n--;

            data.a += 2.0 * n * Math.PI;

これで長方形コースを6周したデータから進行方向 a を求めると、a = 40.11 [rad]くらいになりました。 [deg]に換算すると 4.011 / 3.14 * 180 = 2299.29 [deg] であり、 これを360で割ると 6.3 くらいになるので、カルマンフィルタに与えるセンサデータとして妥当なものになったと思います。

ちなみに、カルマンに食わせるときは地磁気偏角も入れてますよ。

« バッテリの持ち具合 | トップページ | チューニング、チューニング、チューニング »

コメント

コメントを書く

コメントは記事投稿者が公開するまで表示されません。

(ウェブ上には掲載しません)

トラックバック

この記事のトラックバックURL:
http://app.cocolog-nifty.com/t/trackback/167990/46715068

この記事へのトラックバック一覧です: カルマンフィルタに不連続なセンサデータを与えてはいけない:

« バッテリの持ち具合 | トップページ | チューニング、チューニング、チューニング »

最近の写真