.-- --

スポンサーサイト

上記の広告は1ヶ月以上更新のないブログに表示されています。
新しい記事を書く事で広告が消せます。
スポンサー広告 comment(-) trackback(-)
.26 2013

Eigenでカルマンフィルタ理解

こんにちわ。Uedaです。

今日はEigenを使ってカルマンフィルタの検証を行います。

wiki
http://ja.wikipedia.org/wiki/カルマンフィルター

カルマンフィルタを軽く説明します。
wiki曰く、
「誤差のある観測値を用いて、ある動的システムの状態を推定あるいは制御するための、無限インパルス応答フィルターの一種である。」
ということらしいです。

例題から入った方がわかりやすいと思いますので、この度課題とさせて頂いたサイトを紹介
http://satomacoto.blogspot.jp/2011/06/python.html

以下で筆者が実装したプログラムをお見せしますが、このサイトのプログラムをコピペした訳ではないですよ!
なにせい、筆者はPython読めませんw

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

なるほど、問題はロボットの軌跡を推定することです。与えられるものは、ロボットの制御値(例題でいうところの速度(2,2)で動く)と観測値(例題でいうところのGPSによる位置座標)です。これをもとにロボットの軌跡を推定していくわけです。

しかし!!! ロボットの制御にも、観測値にも誤差が含まれます。(どちらか一方が誤差なしなら、そもそも推定する必要がありませんよね

ようは、
制御値だけでは風の影響(誤差)があるので、精度のいい位置が計測できない。
観測値だけではGPSには誤差が含まるので、精度のいい位置が計測できない。
っということで、二つ合わせてもっと精度のいい推定をしよう!このセンサの統合にカルマンフィルタを使おう!ということです。

では、プログラムと検証データです。


#ifdef _kalmanfilter_

const int DATA_COUNT = 10;
// const double NOIZE1 = 2.0;
// const double NOIZE2 = 4.0;
const double NOIZE1 = 4.0;
const double NOIZE2 = 8.0;
std::vector correctList;
std::vector observeList;
std::vector controlList;


const double VEL = 2.0;
double correctX = 0.0, correctY = 0.0;
for (int t = 0; t < DATA_COUNT; t++)
{

correctX += VEL + [self random] * NOIZE1;
correctY += VEL + [self random]* NOIZE1;
double* correct = new double[2];
correct[0] = correctX;
correct[1] = correctY;
correctList.push_back(correct);

double* control = new double[2];
control[0] = VEL;
control[1] = VEL;
controlList.push_back(control);

double* observe = new double[2];
observe[0] = correctX + [self random] * NOIZE2;
observe[1] = correctY + [self random] * NOIZE2;
observeList.push_back(observe);
}

Vector2d mu;
mu(0) = 0.0; mu(1) = 0.0;
Matrix2d sigma = Matrix2d::Zero(2, 2);
Matrix2d Q = Matrix2d::Identity(2, 2);
Matrix2d R = Matrix2d::Zero(2, 2);
R(0, 0) = 2.0; R(1, 1) = 2.0;
for (int i = 0; i < DATA_COUNT; i++)
{
double *correct = correctList.at(i);
double *observe = observeList.at(i);
double *control = controlList.at(i);

// 予測
mu(0) += control[0];
mu(1) += control[1];
sigma += Q;

// 更新
MatrixXd y(2, 1);
y(0, 0) = observe[0] - mu(0);
y(1, 0) = observe[1] - mu(1);
MatrixXd S = sigma + R;
MatrixXd K = sigma * S.inverse();
MatrixXd _mu(2, 1);
_mu(0, 0) = mu(0);
_mu(1, 0) = mu(1);
MatrixXd ans = _mu + K * y;
mu(0) = ans(0, 0);
mu(1) = ans(1, 0);
sigma -= K * sigma;

NSLog(@"%f,%f,%f,%f,%f,%f", correct[0], correct[1], observe[0], observe[1], mu(0), mu(1));
}

#endif


検証データ
カルマンフィルタ

観測値だけの赤いグラフより、カルマンフィルタで推定した緑のグラフの方が、青い実際にロボットが動いた軌跡に近いことがわかります!
カルマンフィルタは、wikiで紹介されている式に当てはめることができれば、異なるデータをブレンドしてよりいい (コーヒーが) 結果を得る (飲める) ことができる。というフィルタです。(ただし、こういったフィルタは他にもいろいろあります〜
関連記事
スポンサーサイト

Comment

Akira
[self random]はどの変数ですか?
2013.11.12 20:19
neoxneo
> [self random]はどの変数ですか?

randamは-1.0~1.0の乱数を発生させるメソッドです。

#define ARC4RANDOM_MAX 0x100000000

...

double r = 0.5 - (double)arc4random() / ARC4RANDOM_MAX;

で、rは-0.5~0.5の範囲の値になるので、

-(double) random
{
return (0.5 - (double)arc4random() / ARC4RANDOM_MAX) * 2.0;
}

を定義して下さい。きっと動きます。
2013.11.13 09:32

Post comment

  • comment
  • secret
  • 管理者にだけ表示を許可する

Trackback

trackbackURL:http://appteam.blog114.fc2.com/tb.php/252-52df74f8

ブログ内検索

関連リンク

製品情報

最新記事

カテゴリ

プロフィール

neoxneo



NEXT-SYSTEM iOS Developers Blog


  • UTO:
    カナダ版iPhone4Sは、マナーモードでシャッター音がならない…


  • Ehara:
    ...


  • Hayate:
    ...


  • Tasaki:
    Developer登録完了...したのはいいけど


  • Ueda:
    ...



リンク

上記広告は1ヶ月以上更新のないブログに表示されています。新しい記事を書くことで広告を消せます。