スポンサーサイト

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

ジャイロ+カルマンフィルタ

今日はジャイロ.
死蔵していたADXRS300と秋月dsPIC33F32JGP202で角速度データを取得しこれにフィルタをかけてみる.


教科書に載っていたトラッキング用のカルマンフィルタでジャイロ角速度をフィルタリングしてみる.
静止時のノイズを観測ノイズとして,エクセルで求めた分散値を入れる.
システムノイズは測り方がよく分からないのでテキトーな値を加速度項に入れてみる.
とにもかくにも行列を逐次計算すれば,なにやらそれっぽい波形が出てくる.
静止時:

手に持って動かしたとき:


ノイズはそれなりに濾されているものの,MEMSジャイロ特有の積分ドリフトはまったく除去されていない.
カルマンフィルタはノイズを平均ゼロの白色ノイズと仮定しているため,DC成分変動は追跡できないためだろうか?
いろいろな文献を見ると,カルマンフィルタはセンサ単体で使うよりも状態方程式の自由度を生かして複数のセンサを融合させると強力だと書いてある.
きっとジャイロ単体ではダメで,加速度センサや地磁気センサなどと組み合わせると,ドリフトが補正されてイケてるデータが取れるのだろう.

カルマンフィルタ以外にもパーティクルフィルタというのも最近流行っているらしい.
今後はこちらも試してみようと思う.
スポンサーサイト

この記事へのコメント

- purin - 2009年09月06日 11:54:00

カルマンフィルタやパーティクルフィルタはどのような教科書を参考にされたのですか?私もこれからジャイロの補正をするので教えていただけたら嬉しいです。

Re - もやね - 2009年09月07日 01:21:21

purinさんこんばんは.
(プリンがお好きな方?)
私もAmazonなんかで探したのですが,カルマンフィルタやパーティクルフィルタの実用的な本は見当たりませんでした.
挙げるとしたら「確率ロボティクス」ぐらいでしょうか.
和訳本なのですが,読むのにちょっと癖があります.
表紙の固い「カルマンフィルタ」「応用カルマンフィルタ」というような本は工学書ではなくて数学書に近くて,とてもすぐにプログラムが作れるような本ではありません.
(「応用」とは名ばかり・・・「よって成り立つ!」で終わる感じ.)
AmazonでKalman,Gyro等のキーワードを入れて洋書をあさるしかないかもしれません.(高いし,英語に苦しみますが・・・)
私自身,まだこれらのフィルタを理解していないのですが,プログラムを作る上で参考にしたのは,WEBページです.
カルマンフィルタは
http://www.cs.unc.edu/~welch/kalman/index.html
のページが分かりやすいです.(カルマンさんの原著論文もあります.読んでも?ですが・・・)
中程にある
http://www.cs.unc.edu/~tracker/media/pdf/SIGGRAPH2001_Slides_08.pdf
のスライドでカルマンフィルタの概要とプログラムの要領が掴めるかと.
スカラー量カルマンフィルタのCプログラム例は
http://www.eonet.ne.jp/~marigold/Lecture/Signal/
こちらの大学の先生が講義メモとして出されているものの最後のほうにあるコードがとても簡単で,実装の手がかりになります.
パーティクルフィルタはWEBに出回っているビジョン系の論文を参考にしました.カルマンに比べて実装は容易だと思います.
パーティクルフィルタは最近流行りの技術のようなのでWEBでキーワード探索するとけっこうHITします.
お試し下さい.

- purin - 2009年09月07日 23:59:16

もやねさん、ご丁寧に教えていただきありがとうございます。プリン好きです(笑)
これから、ジャイロの補正をするところでしたので大変参考になりました。私もジャイロは、「IMU5Degrees 3軸加速度+2軸」を使っています。

観測ノイズ - のざわ - 2009年09月18日 13:04:48

観測ノイズは、たとえば、こんなのを
与えれば良いと思いますよ。
void pred_garch_var(double data[N],double garch_var[N])
{
double var_data[N];
double min,max,mean,mean_plus,mean_minus,par,dir,max_min;
int L;
var_data[0] = 0;
for(L= 0;L < (N-1);L++){
var_data[L+1] = (data[L+1] - data[L])*(data[L+1] - data[L]);
//garch_var[L+1] = var_data[L+1]*var_data[L+1];
// printf("L:%d data:%lf var_data:%lfn",L+1,data[L+1],var_data[L+1]);
}
min = 0;
max = 1;
for(L = 0; L < 50;L++){
mean = (min+max)/2;
mean_plus = mean + mean/100;
mean_minus = mean - mean/100;
dir = mm(var_data,mean_plus) - mm(var_data,mean_minus);
// printf("dir:%lfn",dir);
if(dir > 0){
max = mean;
}else{
min = mean;
}
max_min = max - min;
if(max_min < 0.0001) break;
}
par = (min+max)/2;
//printf("L:%d min:%lf max:%lf par:%lf ",L,min,max,par);
for(L= 1;L < (N-1);L++){
garch_var[L] = par*var_data[L-1] +(1-par)*garch_var[L-1];
}
}
double mm(double data[N],double mean)
{
double var[N];
double Err2;
int L;
Err2 = 0;
var[0] = 0;
for(L = 1;L < N;L++){
var[L] = mean*data[L-1]+(1-mean)*var[L-1];
Err2 += (data[L] - var[L])*(data[L] - var[L]);
}
return(Err2);
}

システム雑音 - のざわ - 2009年09月18日 13:23:38

>>システムノイズは,
先ほどの、状態雑音用のを使っても可能ですが、重いですかね?
推定する状態の過去のモノを集めておいて、これの階差の分散を入れるようにすれば、自動的に集まります。
先ほどの例は、時期によって分散が変化する場合の最近の分散をノイズとしてして与える例の、最も簡単なものです。

- もやね - 2009年09月23日 00:29:16

のざわさん,いろいろ教えていただいてありがとうございます.
まだカルマンフィルタは勉強中なので,よく理解できてなくてすみません.
分散値を時間の関数として逐次求めるやり方ですね.
分散値を一定と置くとゲインがすぐに収束してしまってあまりカルマンらしさが得られなかったので,この方法が使えれば有効と思います.
どうもありがとうございました.

たぶん - のざわ - 2009年09月23日 13:25:06

>あまりカルマンらしさが得られなかった
どうだろう?
可変分散を使うと、余計に早く収束するように思うけど?
見ていないので、正確には、わかりませんが
1)観測ノイズを、可変分散タイプにする。
2)システムノイズを一旦、最適化で求める。
と、やってみて、収束早くて希望するのと違って、ダメっぽい場合は、階差モデルにすれば、ゆっくりしますよ。
Tn = 2*Tn-1 - Tn-2 + vn
Tn-1 = Tn-1
つまり
F
|2,-1|
|1,0|
G
|1,0|
|0,1|
|1,0|
Q
|Q11,0|
|0,Q11|
|R|
ここで、分散を可変分散にする。
こんな感じにすると良いでしょう。

- のざわ - 2009年09月24日 08:49:31

G
|1,0|
|0,1|
ここを間違いました。
G
|1,0|
|0,0|

G
|1|
|0|
これでOKです。

- もやね - 2009年09月24日 20:13:21

のざわさん,詳しい式をありがとうございます.
これは2次のPV(位置,時間)モデルでしょうか?さっそく参考にさせていただきます.
まだカルマンをあまり理解できていないのでいけないのですが,「早く収束した」というのはカルマンゲインのことなんです.
私が試したところ,状態,観測値の分散を一定値のスカラ量とすると,カルマンゲインは状態量や観測量の変動によらず何ステップか後に一定値に収束していきました.
そしてその後は単なる2次のFIRフィルタとして機能している感じだったので,なんか逐次最尤推定っぽくないなと.
すみません,間違ってましたら許してください・・・

- のざわ - 2009年09月25日 12:27:02

>これは2次のPV(位置,時間)モデルでしょうか?さっそく参考にさせていただきます.
いえ、これは階差モデルです。
要するに2,-1って所を0.5,0,5とすれば、普通に移動平均でしょう?
それでも良いのですが、何故こんな2,-1になるかと言えばYt = Yt-1 とすると、Yt-1=Yt-2ですよね?
これをこのまま引くとYt - Yt-1=Yt-1 - Yt-2 になって、左側のYt-1を右に移すとYt=2Yt-1 - Yt-2になるんです。こうすると、早い話が移動平均を取ったような動きになります。
PVで使いたい場合は
X
|P|
|V|
とすると
F=G
|1,1|
|0,1|
|1,0|
とすれば⊿T=1の時のPVモデルです。
>そしてその後は単なる2次のFIRフィルタとして機能している感じだったので,なんか逐次最尤推定っぽくないなと.
所詮ノイズが決まると、その上でのFIRです。
だからノイズを状況によって変化させないと。。と思ったので投稿した訳です。
では。

- もやね - 2009年09月25日 22:50:29

のざわさんこんばんは.
細かい解説をありがとうございます.
せっかく教えていただいたのですが,私が不勉強のためまだ良く理解できてませんです.
もう少しカルマンや信号処理について調べてからご提案の式の意味を考えさせていただきます.

トラックバック

URL :

プロフィール

もやね

Author:もやね
長野県在住の会社員(メカニカル・エンジニア).
ロボットは完全な趣味としてやってます.
E-mail:
mo_ya_ne[a]yahoo.co.jp
[a]⇒@

最近の記事
最近のコメント
最近のトラックバック
月別アーカイブ
カテゴリー
FC2カウンター
ブログ内検索
RSSフィード
リンク
上記広告は1ヶ月以上更新のないブログに表示されています。新しい記事を書くことで広告を消せます。