EKF H matrix

[Peter B]

While making C implementation based on documentation and references you provided I found that some terms in third row of a H(q) matrix have to be different.
While constructing the C(q) matrix you use an assumption that $1 = q_0^2 + q_1^2 + q_2^2 + q_3^2$.
So instead of $q_0^2 - q_1^2 - q_2^2 + q_3^2$ we get $1-2(q_1^2 + q_2^2)$
This is correct representation, but when calculating the partial derivatives of h(t) to make H(q) matrix with gravity reference (0, 0, -1) or (0, 0, 1) we are loosing 2 terms in the 3rd row of H(t).

Here are my calculations:
With gravity reference $g = (0, 0, 1)$

$$h(\hat{q}_t) = [C(\hat{q}_t)^T g] = \left[ \begin{array}{c} 2(q_1 q_3 - q_0 q_2) \\\ 2(q_2 q_3 + q_0 q_1) \\\ q_0^2 - q_1^2 - q_2^2 + q_3^2 \end{array} \right]$$ $$H(\hat{q}_t) = \left[ \begin{array}{c} -2q_2 & 2q_3 & -2q_0 & 2q_1 \\\ 2q_1 & 2q_0 & 2q_3 & 2q_2 \\\ 2q_0 & -2q_1 & -2q_2 & 2q_3 \end{array} \right]$$

Having the 3rd element of $h(\hat{q}_t)$ in form $1-2(q_1^2 + q_2^2)$ we get

$$H(\hat{q}_t) = \left[ \begin{array}{c} -2q_2 & 2q_3 & -2q_0 & 2q_1 \\\ 2q_1 & 2q_0 & 2q_3 & 2q_2 \\\ 0 & -4q_1 & -4q_2 & 0 \end{array} \right]$$

A similar situation is with magnetic reference.

I have tested both version with real data streaming and the results are different.

[Teemu Kumpumäki]

Also tested this one with generated data. In my test the alternative version worked without errors, while the original did produce error while tracking loop.

I did check many papers and did not see any using that 1-2(q2+q2).

I tested with the jacobian from this implementation: https://thepoorengineer.com/en/ekf-impl/

[Mario Garcia]

This is a very interesting point. I will definitely test these two versions. If the one with $q_w^2 - q_x^2 - q_y^2 + q_z^2$ is better, the code (and documentation) will be changed accodringly.

Thanks for bringing this to attention. We all improve this library.

Still, a reason has to be for the difference, even when their definition is mathematically equivalent.

[Peter B]

The thing is that diagonal terms of the rotation matrix depend on every component of a quaternion.
Hence, use of a compact form makes this dependency implicit and leads to wrong derivatives. We do not see those elements, but the dependency still presents.
We must always remember that 1 here is not a mere constant but a specific constraint which contains all components of the quaternion.

[Peter B]

Dear @Mayitzin
Did you have a time to check the above changes?

[Mario Garcia]

Hi! Sorry for the late reply. I have evaluated the versions and I've made the changes in the commit 7ac5851

Here is how I approached the problem:

• I used the dataset of the repository RepoIMU, which contains sensor data of gyroscopes, accelerometers, and magnetometers.
• This dataset also contains measured Quaternions (using a Vicon system), and the data is synchronized to the same time-stamps.

With this data I tested three scenarios:

• Original, which is the implementation of the EKF, as it has been defined up to now.
• Refactored. The method dhdq defines the Jacobian H and has the option of a 'refactored' mode. This refactorization, as explained in the documentation, defines the Jacobian with several matrices, as opposed with a single one. Please see the documentation for further details.
• New, which is the EKF with the Jacobian H defined as suggested by @PeterBorisenko

The errors were measured using the metric Quaternion Angle Difference, also available in the AHRS package.

The results are as follows:

Recording	Original	Refactored	New
TStick_Test02_Trial1.csv	2.403839e-02	2.293890e-02	2.291552e-02
TStick_Test02_Trial2.csv	5.514717e-02	5.167780e-02	5.070877e-02
TStick_Test03_Trial1.csv	7.452721e-02	7.385620e-02	6.918890e-02
TStick_Test03_Trial2.csv	7.594773e-02	7.184042e-02	7.414252e-02
TStick_Test03_Trial3.csv	7.531226e-02	7.229252e-02	7.125031e-02
TStick_Test04_Trial1.csv	1.067118e-01	1.200236e-01	1.051422e-01
TStick_Test04_Trial2.csv	6.259173e-02	6.563081e-02	5.816036e-02
TStick_Test04_Trial3.csv	8.373745e-02	9.052567e-02	7.654144e-02
TStick_Test06_Trial1.csv	1.955587e-01	6.371324e-01	2.015852e-01
TStick_Test06_Trial2.csv	1.974966e-01	1.723569e+00	1.822505e-01
TStick_Test07_Trial1.csv	8.923299e-02	2.704295e-01	6.397840e-02
TStick_Test07_Trial2.csv	1.040668e-01	3.232865e-01	9.499060e-02
TStick_Test07_Trial3.csv	8.938379e-02	2.445449e-01	9.187592e-02
TStick_Test08_Trial1.csv	8.957960e-02	1.394432e-01	3.857306e-02
TStick_Test08_Trial2.csv	4.101043e-02	4.663590e-02	3.618648e-02
TStick_Test08_Trial3.csv	7.104875e-02	8.780755e-02	3.690568e-02
TStick_Test09_Trial1.csv	2.315799e-01	2.501628e-01	5.140734e-02
TStick_Test09_Trial2.csv	1.997696e-01	2.253895e-01	3.861103e-02
TStick_Test09_Trial3.csv	1.492741e-01	1.579206e-01	4.027533e-02
TStick_Test10_Trial1.csv	6.521880e-02	6.539487e-02	6.756707e-02
TStick_Test10_Trial2.csv	6.774476e-02	7.465652e-02	5.302413e-02
TStick_Test10_Trial3.csv	6.020060e-02	6.207642e-02	5.850808e-02
TStick_Test11_Trial1.csv	6.469981e-02	6.649664e-02	6.350834e-02
TStick_Test11_Trial2.csv	6.388582e-02	7.237841e-02	6.302069e-02
TStick_Test11_Trial3.csv	6.513871e-02	6.984346e-02	6.479358e-02

When plotting this information, we get:

A short description using pandas will print:

        Original  Refactored        New
count  25.000000   25.000000  25.000000
mean    0.096116    0.203438   0.071004
std     0.054602    0.342858   0.041352
min     0.024038    0.022939   0.022916
25%     0.064700    0.066497   0.050709
50%     0.075312    0.074657   0.063508
75%     0.104067    0.225389   0.074143
max     0.231580    1.723569   0.201585

We see the refactored mode has the worst performance, with a mean error of ~ 0.2, and a standard deviation of 0.34, while the original version has a mean error of 0.096, and the updated one is equal to 0.07.

Clearly, the updated version performs much better, and I've changed accordingly. Now the Jacobian matrix is defined as:

$$\mathbf{H}(\hat{\mathbf{q}}_t) = 2 \begin{bmatrix} g_xq_w + g_yq_z - g_zq_y & g_xq_x + g_yq_y + g_zq_z & -g_xq_y + g_yq_x - g_zq_w & -g_xq_z + g_yq_w + g_zq_x \\\ -g_xq_z + g_yq_w + g_zq_x & g_xq_y - g_yq_x + g_zq_w & g_xq_x + g_yq_y + g_zq_z & -g_xq_w - g_yq_z + g_zq_y \\\ g_xq_y - g_yq_x + g_zq_w & g_xq_z - g_yq_w - g_zq_x & g_xq_w + g_yq_z - g_zq_y & g_xq_x + g_yq_y + g_zq_z \end{bmatrix}$$

when using the accelerometers, and as:

$$\mathbf{H}(\hat{\mathbf{q}}_t) = 2 \begin{bmatrix} g_xq_w + g_yq_z - g_zq_y & g_xq_x + g_yq_y + g_zq_z & -g_xq_y + g_yq_x - g_zq_w & -g_xq_z + g_yq_w + g_zq_x \\\ -g_xq_z + g_yq_w + g_zq_x & g_xq_y - g_yq_x + g_zq_w & g_xq_x + g_yq_y + g_zq_z & -g_xq_w - g_yq_z + g_zq_y \\\ g_xq_y - g_yq_x + g_zq_w & g_xq_z - g_yq_w - g_zq_x & g_xq_w + g_yq_z - g_zq_y & g_xq_x + g_yq_y + g_zq_z \\\ m_xq_w + m_yq_z - m_zq_y & m_xq_x + m_yq_y + m_zq_z & -m_xq_y + m_yq_x - m_zq_w & -m_xq_z + m_yq_w + m_zq_x \\\ -m_xq_z + m_yq_w + m_zq_x & m_xq_y - m_yq_x + m_zq_w & m_xq_x + m_yq_y + m_zq_z & -m_xq_w - m_yq_z + m_zq_y \\\ m_xq_y - m_yq_x + m_zq_w & m_xq_z - m_yq_w - m_zq_x & m_xq_w + m_yq_z - m_zq_y & m_xq_x + m_yq_y + m_zq_z \end{bmatrix}$$

when using accelerometers and magnetometers.

Thanks for bringing this topic. If there are further questions, feel free to ask them here. Otherwise, we can close this issue.

[Mario Garcia]

I think we can confidently close this issue. The changes have been implemented in code and in the docstrings generating the documentation.