Implementing Kalman filter

[Adixylian]

Hello guys

I want to make UAV so I have to filter data. OK, I understand how Kalman filter works and else.
So I made one. I tested it and it gave me very good numbers, compared to raw sensor data (gyroscope, magnetometer, accelerometer...)

But there is a problem, it takes 350 miliseconds for it just to go through one loop of the filter, for one axis of motion.
And that is way too long.
So I wonder, has anybody managed to use Kalman filter with this microcontroller (3x3 matrices) and has anybody got an advice
how to shorten that time?
Because, you know, 350 ms is far too long time and it would lead to very unstable system

[mianos]

Are you using a C core and calling it from python, or is it all in python? Last time I did one I hacked the fixed point one for a non FPU CPU and it ran a lot faster than 350mS (but lacked some precision ) but the pyboard should be able to do floats pretty well.

[Adixylian]

It's all in python... If you have a solution, please share it with me

[JonHylands]

It would be much simpler if you show us your code, and then we can look at it and do some form of analysis.

I have inverse kinematics code running on MicroPython, and doing a full IK calculation for a 4-legged (3 DOF per leg) robot runs in 4ms. That involves a whole pile of floating point math and trig functions.

- Jon

[blmorris]

@pythoncoder has put together a library to do sensor fusion on the pyboard in MicroPython. It uses the Madgwick algorithm (quaternions); I've just started using it, and it seems to work pretty well - certainly faster than 350 mS.

https://github.com/micropython-IMU/micropython-fusion

-Bryan

[Adixylian]

Thanks, I'll check that and I will send you code when I get home.
One more thing, just to find the inverse of (3x3) it takes it about 40ms, but for (2x2) it is cca 15ms, which is stll too long. I'm using ulinalg and umatrix libraries from github

[pythoncoder]

@Adixylian I'm no maths guru but I have done some reading on sensor fusion. I gather quaternions are regarded as the best technique because they avoid the problem of "gimbal lock". The Madgwick algorithm seems to be used by most UAV people which is why I ported it to MicroPython. An update takes 1.6mS. Most UAV control loops run at 10-50Hz, so even at the highest rate the filter will only use 8% of the CPU.

[Adixylian]

Wow if it's really like that I'll be the happiest man on this planet. I'll try it today. Many thanks