Code is here. http://www.thdzero.com/Kalman/ 56k users beware.
I was sick and tired of our drifting gyro, and our motor-affected magneto, so I wrote a kalman filter to be run on an Atmel AVR ATMega64, this will come later. This takes gyroscope input as main, with compass as a reference value to keep both in check. If your gyro is noisy, or your compass drifts, you’re out of luck, otherwise, this filter should help alot.
This filter resists godawful amounts of noise and gyroscope drift, hopefully it’ll give us smooth nav. If anyone else wants to convert it to whatever flavor of C for their embedded processor and use it, feel free.
N.B. I didnt have the RC in mind when I wrote this, it may be difficult to have a well-working kalman filter without high sample rate and high processing power.
I’m still working on having the filter ignore momentary spikes (see last 2 graphs) which would be caused by sudden motor reversal. If anyone has ideas, I’d love to hear them. Low pass filtering the compass input helped but its not enough to smooth over the entire section. Im thinking of maybe jacking up the Sz value if and only if the compass and gyro disagree.