pose derivation in FusionAhrsUpdate algorithm about fusion

[xioTechnologies]

Variables such as halfGravity and halfGyroscope exist as an optimisation to avoid unnecessary operations. There any many intermediate calculations that involve a multiplication by 2. The optimisation removes all of these intermediate multiplications, and replaces them with a single multiplication at the end.

For example, consider the following sequence:

1. value = 2 * input
2. value = value + 2 * constant_a
3. value = value + 2 * constant_b
4. output = value

This can be optimised as:

1. half_value = input
2. half_value = half_value + constant_a
3. half_value = half_value + constant_b
4. output = 2 * half_value

This is effectively what is going on in FusionAhrsUpdate.

[jacklee0575]

Thank you for your kind reply of the detailed trick in the code. I also want to ask the second question. I have tested the algorithm without the feedback of acc or mag, and contrasted with the direct integration of the gyro data. The results show that the quaternion implementaion with a half gyro is more stable than the direct integration of single axis gyro data, which has less drift over time while the direct integration caused drift is very obvious. Have you noticed this phenomenon? Could you tell me the possible reason for this phenomenon. Many thanks!

[xioTechnologies]

Calling FusionAhrsUpdate with the arguments accelerometer and magnetometer set to FUSION_VECTOR_ZERO will remove the accelerometer and magnetometer feedback so that the orientation is calculated though integration of the gyroscope data.

It sounds like you are comparing this result for a triple-axis gyroscope, to different result for a single axis gyroscope. The difference in performance will be because the single-axis is not accounting for motion (even if very small) in the other two axes.

[jacklee0575]

Thank you for your detailed reply. I have understand this phenomenon.