Thanks to Mark, and an outer-blog-space e-mail conversation I had with him, I think now I have just enough interest and need for a Kalman filter that I’ll do some proper research and thinking. I’ll explain why but first, a recap on the complementary filter:
Angles of pitch and roll can be calculated from both the gyro and the accelerometer. For the gyro, you integrate the rotation rate; for the accelerometer, you assume the only force is gravity and use the distribution of gravity across the sensors plus a little trigonometry.
Both methods have flaws
- the gyro results drift – I never have found out why; whether it’s integration errors, or inherent to the gyro itself, perhaps due to its angular momentum. Regardless of which way, you can only trust the angles from its integration short term.
- the accelerometer results are plagued with noise and cease to be accurate when forces other than gravity come into play i.e. acceleration from the props and drag / wind resistance, but longer term, these average out, yielding accurate values.
The complementary filter does a rolling ‘merge’ of these values, with a short-term bias on the gyro against a long term background provided by the accelerometer.
This has worked very well until I moved away from controlling Phoebe with angles to controlling her with motion – the reason behind the move (pun intended) being angles could never be used to counteract drift in windy conditions.
Anyway, how could the Kalman filter do a better job? It provides an iterative way to filter out noise and hone in on the ‘correct’ value in a predictive way. There, you now know exactly the same as I do!
That means I can stop using the gyro for angles, retiring it into just a rotational stability role.
The way the Kalman approach would work is a bit like this: you may remember the code loops at about 450Hz, but only processes the resultant integrated / averaged data at about 40Hz currently? If instead of averaging @ 450Hz, each accelerometer sensor read is fed through a Kalman filter then at the 40Hz processing point, it’s the refined value from Kalman filter that is used for motion processing. That should remove noise and reduce the effect of non-gravitational forces on the sensor output.
2 problems I see initially:
- the basic Kalman filter requires inputs at a fixed frequency; that’s fine as that’s how the MPU6050 makes its data available. But currently I have it set to 1kHz to make sure every sensor read is new but that means sometimes the code loop running at 450Hz will miss an output from the sensors – not a problem for integrating as I track sample time closely; but for Kalman I’d have to drop the 1kHz down to 333Hz to ensure the values fed into the Kalman are all equally spaced at 3ms – not really a problem except I’ve worked so hard to get the loop speed as high as possible, and now I don’t need that!
- More importantly, I really don’t understand how the filter works in any detail whatsoever. The code I’ve seen is simple but until I understand what it’s doing and how it works in intimate detail, I’m not letting it anywhere near Phoebe.
Here’s a few of the links Mark pointed me at: