Re: Kalman Filter Code
Thanks for the code. Could you make a recommendation as to what type
of gyro would work the best with this code? I have a sparkfun
gyro/accelerometer duo board. Also, I am having a problem
understanding the structure of the source code files. Perhaps you
can explain this a bit. Where does the program start and how does it
branch? Have you succeeded in building a balancing bot using this
code? Thanks a lot. --ray
--- In b..., "camel85kv" wrote:
> If any of you have ever considered building a two wheel
> robot, here is some code that I have written.
> It samples an angular rate gyroscope about 150 times per
> and runs a background task to sample an accelerometer. The
> acceleromter is filtered by sampling 5 times for each reading.
> time a sensor reading is ready, the data is sent to a modified two
> dimentional kalman filter, which ultimately returns the tilt angle
> the system in the form of the global variable KalmanAngle.
> The filter subs are set up so that sensor readings do not have
> come in at the same time. This is what allows the incredible rate
> 150 tilt outputs per second. As it turns out, the gyroscope is
> 150 times per second, and I estimate that the accelerometer is read
> about 30 or so times per second.
> I optimized the matrix operations (and the rest of the code) in
> the kalman filter so much that the original formulas are totally
> non-existant. In this setup, many of the terms are 0, so I left
> out. It should still be readable enough for anyone to adjust the
> constants for their particular application.
> I hope that this is not too off topic, but I thought someone
> there may want to use this. To my knowledge, it is the first
> implementation of kalman filters, specifically for balancing
> on the BasicX-24. See David P. Anderson's "Nbot" or Larry
> "Gyrobot" if you don't know what I am talking about.
> -Kyle Vogt
code was posted in 2003. I can help answer some of your questions.
Sparkfun's accelerometer/gyro breakout boards should work fine with
Kyle's code, but you will need to make changes to change from PWM
input to analog input.
His code will compile as posted, I believe; execution starts at
Main(). He runs two tasks, the main task that spins and collects rate
gyro data and another task that spins getting acceleration data.
You'll find that some values are fudged, some hard numbers are where
variables should be, and other signs of incomplete code. I found that
it fundamentally works, but you'll need to integrate it into your device.
Kyle didn't indicated if this was successful in a project, but I
suspect it was.