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

Math errors in AQUA filter

tborensztejn opened this issue · comments

To calculate quaternion using gyroscope by integration of measurement, the factor is not 1/2 but -1/2 (sign error).
qDot = -0.5*self.Omega(gyr) @ q # Quaternion derivative (eq. 39)

To calculate quaternion for accelerometer and magnetometer (no gyroscope):

if lx >= 0:
q_mag = np.array([np.sqrt(Gamma+lxnp.sqrt(Gamma))/np.sqrt(2Gamma), 0.0, 0.0, ly/(np.sqrt(2)np.sqrt(Gamma+lxnp.sqrt(Gamma)))])
else:
q_mag = np.array([ly/(np.sqrt(2)np.sqrt(Gamma-lxnp.sqrt(Gamma))), 0.0, 0.0, np.sqrt(Gamma-lxnp.sqrt(Gamma))/np.sqrt(2Gamma)])

Missing parentheses at denominator: a/bc =/ a/(bc)

I found so many bug during the C rencoding process. You should check your code using simulate script of an IMU or use it in real implementation...