Today I accomplished a big speed improvement of my IMU filter. The filter frequency used to be maximal 140 Hertz. It now is 240 Hertz.
What is the effect of this speed improvement you might wonder? Well the main thing is that one iteration of the filter takes less processing time. This leaves more processing time for other processes. Another effect is that the integration error is less. So the filter should be more accurate.
How did I realize the speed improvement? The filter uses vectors and matrices and a lot of lineair algebra. Before I had a 3×3 matrices based on a 2 dimensional array of 3×3 elements and vectors based on one dimensional arrays of 3 elements. I replaced the arrays with variables and I got rid of the loops around the arrays. So what used to be something like this:
For (int i=0;i<3;i++)
Now has become something like this:
It makes the code ugly and a bit harder to maintain. But who cares.
Soon to come. Compass integration in the IMU filter! This makes it a 9DOF filter and will prevent filter drift over the Z-axis.