ジャイロセンサーで得られたquaternionで加速度を基準座標系に戻し、位置推定のテストのプログラムをpythonで作成しております。
重力加速度の除去がうまくいかず困っております。
あくまでも推定値であることを前提に置いた上で、以下ローパス・ハイパスフィルタリングを行って重力加速度の除去の方法があると知りました。
以下でプログラミングを行なってみました。
python
1#加速度データをacx,acy,aczとする。quaternionを用いて基準座標系に計算済 2#ローパスフィルタ:時系列のデータの平滑化 3#b:平滑化係数(0<b<1)平滑化したい場合はなるべくbの値は小さくする 4#low(x,y,z)の初期値を設定する。 5low_x=0 6low_y=0 7low_z=0 8#bを平滑化係数とする 9 10#一つ前の加速度 11oldaccel_x=0 12oldaccel_y=0 13oldaccel_z=0 14#一つ前の速度 15oldspeed_x=0 16oldspeed_y=0 17oldspeed_z=0 18 19#速度,距離の初期値を設定する。 20speed_x=0 21speed_y=0 22speed_z=0 23difference_x=0 24difference_y=0 25difference_z=0 26#平滑化係数を0.1とする 27b=0.4 28#測定間隔 t=1/30 29diff_X=[] 30diff_Y=[] 31diff_Z=[] 32 33for i in range(len(acx)): 34 #ローパスフィルタリング 35 #ローパスフィルター(現在の値 = 係数 * ひとつ前の値 + (1 - 係数) * センサの値) 36 low_x=low_x*b+acx[i]*(1-b) 37 low_y=low_y*b+acy[i]*(1-b) 38 low_z=low_z*b+acz[i]*(1-b) 39 #ハイパスフィルタリングを行う 40 #ハイパスフィルター(センサの値 - ローパスフィルターの値) 41 highpass_x=acx[i]-low_x 42 highpass_y=acy[i]-low_y 43 highpass_z=acz[i]-low_z 44 #速度計算(加速度の台形積分) 45 speed_x=((highpass_x+oldaccel_x)*t)/2+speed_x 46 oldaccel_x=highpass_x 47 speed_y=((highpass_y+oldaccel_y)*t)/2+speed_y 48 oldaccel_y=highpass_y 49 speed_z=((highpass_z+oldaccel_z)*t)/2+speed_z 50 oldaccel_z=highpass_z 51 #変位計算 52 difference_x=((speed_x+oldspeed_x)*t)/2+difference_x 53 oldsppeed_x=speed_x 54 difference_y=((speed_y+oldspeed_y)*t)/2+difference_y 55 oldsppeed_y=speed_y 56 difference_z=((speed_z+oldspeed_z)*t)/2+difference_z 57 oldsppeed_z=speed_z 58 diff_X.append(difference_x) 59 diff_Y.append(difference_y) 60 diff_Z.append(difference_z) 61 62 63コード
この方法を用いるとデータが大きくずれるのですが、手法がまちがっておりますでしょうか。
この方法を用いて基準座標系から計算以外に
G=[1,0,0,-9.80665]、もしくはG=(0,0,-9.80665)として重力をquaternion表記して計算していく手法などをみかけました。
ノイズなどの問題もあり、あくまでも推定値での計算となるとは思われますが、重力加速度を加速度から除去する際の手法について望ましいと思われる方法などはありますでしょうか。
回答1件
あなたの回答
tips
プレビュー
バッドをするには、ログインかつ
こちらの条件を満たす必要があります。