gareth-cross
commented
4 years ago @tdkhan Thank you for your suggestion. To be honest, I created this repo some time ago when I was in school. I do not consider it a good reference implementation of an attitude filter, and were I to re-write it today the code would look different in many respects. It is entirely plausible to me that there are mistakes in the implementation.
This code is not maintained, and I would encourage developers to look elsewhere for a better implementation. (Nor is it remotely tested to the standard I would now demand, even for my own personal use).
As to whether or not the rotation is transposed in the expression prediction = R[.transpose] * gravity, it depends on if the filter is parameterized as R = world_R_imu or R = imu_R_world. It appears when I first wrote it, I chose imu_R_world, with the tangent space defined in the right (world) frame. This is an example of something I might do differently today.
The choice of parameterization and tangent space must be consistent throughout the implementation, including the process and measurement model. It is very possible one (or both) of these steps is currently incorrect.
However, as I am no longer maintaining this implementation (or adding the required tests), I will instead preface the README w/ the warning above, link to this ticket, and archive the repository. Thank you all the same for your contribution.
Hi Gareth,
I have been working on my own 6-axis IMU sensor fusion ekf implementation, and your project provided a very good reference to validate against. My project is a python class implementation unlike yours, however, the math is very similar. Whilst implementing the update step of the ekf I noticed some rotational math that I believe is incorrect. The computation of the predicted gravity vector in line 66 of AttitudeESKF.cpp file should be the other way round. Instead of multiplying it with rotation matrix R, it should be multiplied by the transpose (inverse) of the rotation matrix. This makes sense since the gravity vector does not get rotated but rather the inertial frame it is described in is rotated (change of basis). I have validated it in my implementation with a set of IMU data and found this to be correct.
Subsequently, the determination of observation matrix H must be changed in line 69 of the same file. The multiplication should be with R.transposed() instead of R. Also, the rotation matrix needs to be multiplied with the gravity vector and then the cross-skew matrix should be calculated. I have tested this with rotations across different axis with angles ranging from 90 degrees to 360 degrees and found my results to be correct.
I'll fork this repository and make the proposed changes and then open a pull request. Please let me know your thoughts and correct me if I have missed something.
Thanks, Taimoor