はじめに
madgwick filterをいろいろいじくりまわしてみたので報告連絡相談。madgwick filterの前に社会性filterを実装したほうがいいのでは？
結果
角速度計測値オフセットの有無による違い
以下のように、x軸に1Hz正弦波を入力した結果を示す。角速度にはガウシアンノイズを付加しており、上のグラフではノイズ平均値が0[rad/s]、下のグラフは0.0175[rad/s]となっている。
オフセットを含むと時間経過とともに姿勢推定結果がずれていく様子が確認できる。
センサ設置ミスアライメントの有無による違い
センサの回転軸からZ軸上方向に10mmオフセットさせるとどうなるか確認した。結局加速度はとなるので、角速度が速いときに影響を受けやすい。
1Hzくらいだとあんまり変わらない。
ソースコード
import matplotlib.pyplot as plt import numpy as np def e2q(roll, pitch, yaw): #ZYXオイラー→クォータニオン(qw qx qy qz) qx = np.sin(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) - np.cos(roll/2) * np.sin(pitch/2) * np.sin(yaw/2) qy = np.sin(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) + np.cos(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) qz =-np.sin(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) + np.cos(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) qw = np.cos(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.sin(pitch/2) * np.sin(yaw/2) return [qw, qx, qy, qz] def q2j(r0, r1, r2, r3): #qdotを示すためのヤコビ行列 output = np.zeros([4,3],dtype=float) output = np.array([[-r1/2, -r2/2, -r3/2], [ r0/2, -r3/2, r2/2], [ r3/2, r0/2, -r1/2], [-r2/2, r1/2, r0/2] ]) return output def q2r(r0, r1, r2, r3): #クォータニオン(qw qx qy qz)→回転行列 output = np.zeros([3,3],dtype=float) output = np.array([[1-2*(r2**2+r3**2), 2*(r1*r2-r0*r3), 2*(r1*r3+r0*r2)], [ 2*(r1*r2+r0*r3), 1-2*(r1**2+r3**2), 2*(r2*r3-r0*r1)], [ 2*(r1*r3-r0*r2), 2*(r2*r3+r0*r1), 1-2*(r1**2+r2**2)] ]) return output def q2e(r0, r1, r2, r3): #クォータニオン(qw qx qy qz)→ZYXオイラー rot = q2r(r0, r1, r2, r3) y = np.arcsin(-rot[2,0]) if np.abs(np.cos(y)) > 0: x = np.arctan(rot[2,1]/rot[2,2]) z = np.arctan(rot[1,0]/rot[0,0]) else: x = 0 z = np.arctan(-rot[1,0]/rot[1,1]) return x,y,z def q2jh(r0, r1, r2, r3): #qに関する修正項を示すためのヤコビ行列 jh = np.array([[-2*grav*r2, 2*grav*r3,-2*grav*r0, 2*grav*r1], [ 2*grav*r1, 2*grav*r0, 2*grav*r3, 2*grav*r2], [ 2*grav*r0,-2*grav*r1,-2*grav*r2, 2*grav*r3] ]) output = np.dot(np.linalg.pinv(np.dot(jh.T,jh)),jh.T) return output def q2jo(r0, r1, r2, r3): #omgb(角速度バイアス)に関する修正項を示すためのヤコビ行列 jo = dt*q2j(r0, r1, r2, r3) output = np.dot(np.linalg.pinv(np.dot(jo.T,jo)),jo.T) return output #初期設定############################################################################## dt = 0.001#時間ステップ end_time = 1#計算終了時間 input_omg = 10.0*2.0*np.pi#入力角速度 input_ang = 10/180*np.pi#入力振幅 gam = 0.006#縮小因子ガンマ off_r = np.array([0,0,0])#センサ設置ミスアライメント wtime = 0.0#無駄時間 grav = 9.8#重力加速度 # ノイズ生成 mean_omg = 1/180*np.pi#角速度センサノイズ平均（定常偏差） sigma_omg = 1/180*np.pi#角速度センサノイズ（シグマ） noise_omg = np.random.normal(mean_omg, sigma_omg, [int(end_time/dt),3]) mean_acc = 0 * grav#加速度センサノイズ（定常偏差） sigma_acc = 0.1 * grav#加速度センサノイズ（シグマ） noise_acc = np.random.normal(mean_acc, sigma_acc, [int(end_time/dt),3]) #初期設定ここまで############################################################################## #初期化############################################################################## x_s = np.zeros(int(end_time/dt),dtype=float) off_s = np.zeros([int(end_time/dt),3],dtype=float) omg_s = np.zeros([int(end_time/dt),3],dtype=float)#ジャイロセンサー値 ang_s = np.zeros([int(end_time/dt),3],dtype=float)#角度真値 acc_s = np.zeros([int(end_time/dt),3],dtype=float)#加速度センサー値 grav_e = np.zeros(3,dtype=float)#重力による加速度 grav_e[0] = 0 grav_e[1] = 0 grav_e[2] = grav for i in range(int(end_time/dt)): x_s[i] = i*dt#時刻 omg_s[i,0] = input_ang * input_omg*np.cos(input_omg*i*dt)#センサー値入力 omg_s[i,:] = omg_s[i,:] + noise_omg[i,:]#ノイズ付加 ang_s[i,0] = input_ang * np.sin(input_omg*i*dt)#角度真値 quat_tmp = e2q(ang_s[i,0], ang_s[i,1], ang_s[i,2]) rot_tmp = q2r(quat_tmp[0], quat_tmp[1],quat_tmp[2],quat_tmp[3]) off_s[i,:] = np.dot(rot_tmp,off_r)#ミスアライメント考慮したセンサ位置 acc_s[i,:] = grav_e + np.cross(omg_s[i,:],off_s[i,:]) + noise_acc[i,:]#センサー値入力+ミスアライメントによる遠心力+ノイズ付加 acc_buf = np.zeros([int(end_time/dt),3],dtype=float)#加速度センサー値 acc_buf[int(wtime/dt):int(end_time/dt)] = acc_s[0:int(end_time/dt)-int(wtime/dt)].copy() acc_s = acc_buf.copy() quat = np.zeros(4,dtype=float)#現在のクォータニオン omga = np.zeros(3,dtype=float)#現在の角速度計測値 omgb = np.zeros(3,dtype=float)#現在の角速度バイアス jq = np.zeros([4,3],dtype=float) x = np.zeros(7,dtype=float) quat = e2q(ang_s[0,0], 0, 0) x[0:4] = quat x[4:7] = omgb save_data = [] #初期化ここまで############################################################################## #計算開始 for i in range(int(end_time/dt)): #予測ステップ omga = np.array([omg_s[i,0],omg_s[i,1],omg_s[i,2]])#現在の角速度計測値 取得 jq = q2j(x[0], x[1], x[2], x[3]) A = np.vstack([np.hstack([np.eye(4,dtype=float),dt*jq]),np.zeros([3,7],dtype=float)]) B = np.vstack([dt*jq,np.zeros([3,3],dtype=float)]) x = np.dot(A,x) + np.dot(B,omga) #修正ステップ jh = q2jh(x[0], x[1], x[2], x[3]) jo = q2jo(x[0], x[1], x[2], x[3]) rot_tmp = q2r(x[0], x[1], x[2], x[3]) err = grav_e - np.dot(rot_tmp.T,acc_s[i,:]) delta_q = np.dot(jh,err.T) delta_o = np.dot(jo,delta_q) C = gam * np.hstack([delta_q,delta_o]) x = x + C #クォータニオンからZYXオイラー角に変換 [ex,ey,ez] = q2e(x[0], x[1], x[2], x[3]) save_data.append([ex,ey,ez]) save_data_np = np.array(save_data, dtype=float) fig = plt.figure() title_name = r'$\gamma$'+"="+str(gam) + ", " + r'$\omega$'+r'$_{offset}$'+" ="+'{:.3g}'.format(mean_omg) +"[rad/s]" + ", " + "wasted time ="+'{:.3g}'.format(wtime) +"[sec]" # 1行2列の1番目 a1 = fig.add_subplot(3, 1, 1) a1.plot(x_s, ang_s[:,0]*180/np.pi, label="true") a1.plot(x_s, save_data_np[:,0]*180/np.pi, label="filted") a1.grid() a1.set_title(title_name) a1.set_xlabel('time[sec]') a1.set_ylabel('angle[deg.]') a1.legend() a2 = fig.add_subplot(3, 1, 2) a2.plot(x_s, omg_s[:,0], label="x") a2.plot(x_s, omg_s[:,1], label="y") a2.plot(x_s, omg_s[:,2], label="x") a2.grid() a2.set_xlabel('time[sec]') a2.set_ylabel('angular vel[rad/s]') a2.legend() a3 = fig.add_subplot(3, 1, 3) a3.plot(x_s, acc_s[:,0], label="x") a3.plot(x_s, acc_s[:,1], label="y") a3.plot(x_s, acc_s[:,2], label="x") a3.grid() a3.set_xlabel('time[sec]') a3.set_ylabel("acc[m/"+r'$s^{2}$'+"]") a3.legend() # a2.set_title("B") plt.show() fig.savefig("gam="+str(gam)+"omg="+'{:.3g}'.format(mean_omg)+"wtime="+'{:.3g}'.format(wtime)+".png")
