Mayitzin / ahrs

Attitude and Heading Reference Systems in Python

Home Page:https://ahrs.readthedocs.io/

Geek Repo:Geek Repo

Github PK Tool:Github PK Tool

Two calculation models leadind to different results

fenaux opened this issue · comments

Thanks for making this package available

I use Madgwick filter in two different modes
first one
quat = np.zeros((time_run.size, 4), dtype=np.float64)

"""initialize filter"""
madgwick = ahrs.filters.Madgwick(frequency=fs)
""" initial state"""
quat[0] = np.array([q0[0], q0[1], q0[2], q0[3]])
gyr0=np.zeros(3, dtype=np.float64)

"""For all data"""
for t in range(1, time_run.size):
quat[t] = madgwick.updateIMU(quat[t-1], gyr=gyr_run[t] * np.pi/180, acc=acc_run[t])

acc_earth = []
gyr_earth = []
for acc_t, gyr_t, q_t in zip(acc_run, gyr_run, quat):
acc_earth.append(Quaternion(q_t).rotate(acc_t))
gyr_earth.append(Quaternion(q_t).rotate(gyr_t * np.pi / 180))
acc_earth = np.array(acc_earth)
gyr_earth = np.array(gyr_earth)
acc_earth -= np.array([0,0,1]) # remove gravity
acc_earth *= 9.81 # switch to m/s/s

second one
attitude = ahrs.filters.Madgwick(acc=acc_run, gyr=gyr_run * np.pi / 180, q0=q0, fs=fs)
acc_2 = []
for acc_t, q_t in zip(acc_run, attitude.Q):
acc_2.append( Quaternion(q_t).rotate(acc_t) )
acc_2 = np.array(acc_2)
acc_2 -= np.array([0,0,1]) # remove gravity
acc_2 *= 9.81 # switch to m/s/s

and results are not the same
image

as the person wearing the IMU is bending without changing place around 27s, result of mode 2 seems to be more realistic
But why are these two modes leading to different results ?