Heading estimation in tilt.py wrong
davidradakovits opened this issue · comments
The calculation of the tilt-compensated heading, which is probably taken from NXP's AN4248 is incorrect. since the rotation matrices of the negative angles phi (roll) and theta (pitch) need to be computed, but the positive angles are fed. This error was carried over from the NXP application note.
Basically, while the rotation was defined correctly as b = R_y(-theta) * R_x(-phi) * m, the following steps where taken with positive angles.
Therefore
b_x should be m_x * cos(theta) + m_y * sin(theta) * sin(phi) - m_z*sin(theta)*cos(phi)
b_y should be m_y * cos(phi) + m_z * sin(phi)
b_z should be m_x * sin(phi) - m_y * cos(theta) * sin(phi) + m_z * cos(theta) * cos(phi)