Math errors in AQUA filter

[tborensztejn]

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...

[Mayitzin]

Hi, thanks for pointing this out.

However, I couldn't find a problem with the mentioned expressions, and I think I know why the confusion appears: the authors use a different convention. In the section 5.1. Prediction we read:

the quaternion derivative from an angular rate measurement is usually calculated for the forward quaternion, that is, the one representing the orientation of the local frame with respect to the global frame. Because in this article we use the inverse orientation, for the sake of clarity, we compute the quaternion derivative for our convention

The forward quaternion derivative (eq. 37) is defined in the article as:

$$_L^G\dot{\mathbf{q}}_{\omega, t_k} = \frac{1}{2}\,_L^G\mathbf{q}_{t_{k-1}}\otimes ^L\mathbf{\omega}_{q, t_k}$$

But, in AQUA's case they need the derivative of the inverse unit quaternion defined as:

$$_G^L\dot{\mathbf{q}}_{\omega, t_k} = -\frac{1}{2}\,^L\mathbf{\omega}_{q, t_k} \otimes _G^L\mathbf{q}_{t_{k-1}}$$

A couple of things to notice here:

• The operation is not a normal matrix multiplication, but a Hamilton product (denoted with $\otimes$) used to get the product of the two quaternions $\mathbf{q}_t$ and $\mathbf{\omega}_q$.
• To "invert" the orientation of the derivative, the authors swapped the order of the operands and inverted the sign of the product. For further details about this change see page 15 (numbered page 19316) in the article.

To linearize this operation, they redefine the derivative in matrix form:

$$_G^L\dot{\mathbf{q}}_{\omega, t_k} = \mathbf{\Omega}(^L\mathbf{\omega}_{t_k}) _G^L\mathbf{q}_{t_{k-1}}$$

Here is where the confusion arises.

It is true that the product has to be negated, so that we can obtain the expected inverse derivative. However, the elements in the $\Omega(\omega)$ matrix are already negated, per original definition:

$$\mathbf{\Omega}(^L\mathbf{\omega}_{t_k}) = \frac{1}{2} \begin{bmatrix} 0 & \mathbf{\omega}^T \\ -\mathbf{\omega} & -\lfloor\mathbf{\omega}\rfloor_\times \end{bmatrix} = \frac{1}{2} \begin{bmatrix} 0 & \omega_x & \omega_y & \omega_z \\\ -\omega_z & 0 & \omega_z & -\omega_y \\\ -\omega_y & -\omega_z & 0 & \omega_x \\\ -\omega_x & \omega_y & -\omega_x & 0 \end{bmatrix}$$

Admittedly, I also got confused initially, because the article omits the product with $\frac{1}{2}$, but we can confirm this definition with other sources like the recommended read Quaternion kinematics for the error-state Kalman filter.

Additionally, I tested the change from $\frac{1}{2}$ to $-\frac{1}{2}$, as suggested, against the recordings of the synchronized dataset RepoIMU, but the results quickly diverged and the estimations were clearly worse.

This confirmed my initial hypothesis that the definition of the quaternion derivative has been correctly set.

Regarding the quaternion from magnetometer as you mentioned. I assume you refer to the denominators dividing $l_y$ at each definition in the method estimate().

Here I couldn't find any missing parenthesis. Both denominators are surrounded entirely by parenthesis, and performing as expected.

I refactored the code, so that each element is defined per line. Maybe, someone can double check that the definitions are ok.