SparkFun Forums

[mpthompson]

I'm attempting to implement a Kalman filter for the SparkFun IMU 5 Degrees of Freedom to get somewhat accurate pitch and roll information from the device. My ultimate plan is to attach it to an AVR ATmega168 and have it present the pitch and roll information over an I2C interface. I'm implementing the Kalman filter as described at the following URLs:

http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data
http://academic.csuohio.edu/simond/cour ... kalman.pdf

My code is based on the linked embedded systems article linked above and can be found here:

http://home.comcast.net/~michael.p.thom ... man_test.c

I realize the code as implemented is hopelessly inefficient to run on an AVR microcontroller, but for now I want to get some confidence I can get the algorithm working and then optimize the hell out of it later. For now I'm feeding samples gathered over several seconds on the AVR microcontroller as static data to the kalman_update() function. The output looks somewhat reasonable, but perhaps I'm being to optimistic. I'm just about a day into learning about gyros, accelerometers and combining inputs through the Kalman filter.

The main question I have at this point is how to determine the values for the measurement noise covariance value Sz and the process covariance matrix Sw for SparkFun IMU 5 unit. I'm not even really certain at all what the Sw matrix would look like. The articles I'm referencing basically say this has to be found by doing experiments. Can anyone provide me some tips on way I might determine these values?

Any suggestions would be very much appreciated. I'll be willing to share the final results of the project with the community here as I think there might be interest in a Kalman filter on an AVR MCU attached to the IMU 5. Hopefully I'm not being overly optimistic, but it hasn't stopped me before .

-Mike

[emf]

I can't answer your question, but I'm working on a similar project too. I was planning on spending most of the weekend on it last week, but I wound up playing with some new dsPICs I just got instead. Maybe I'll focus a bit more this weekend.

Ghint said he was also working on converting the code he has to for the 6DOF unit to work with a 5DOF IMU, so I imagine one of us will get something working pretty soon.

How did you get the sample data at the top of your file? Is the angle part from some external measurement or is it derived from the accelerometer?

[mpthompson]

The generated data was captured directly on an AVR every 1/50th from the IMU 5 and dumped directly to the serial port. This specific data was captured as I manually tilted the IMU to about -90 degrees on the X-axis and then back again.

The accelerometer data was pre-processed to angular data as described here:

http://tom.pycke.be/mav/69/accelerometer-to-attitude

And the gyroscope data was pre-processed to degrees/second rate information as described here:

http://tom.pycke.be/mav/70/gyroscope-to ... ch-and-yaw

If found the methods outlined these two links to work out pretty well. At least the data looks reasonable to me.

-Mike

[mpthompson]

For anyone interested, an optimized version of the Kalman filter code I'm creating can be found here:

http://home.comcast.net/~michael.p.thom ... an_test2.c

I have a further refined version of this code functioning on an AVR ATmega168 operating at 20MHz to determine pitch and roll from the IMU 5 every 1/10th of a second. If anyone is interested, I can post the AVR Studio project when I return from vacation later in February. The code seems to be functioning fairly well given the relatively small amount of development effort I have put into it so far. However, the filter parameters certainly need to be further tuned as well as the code further optimized to work within the very limited computational resources of the 8-bit AVR MCU.

I hope someone else finds this useful as I would like to work collaboratively with some other people to get the IMU 5 working as well as possible for robotic balance and other applications.

-Mike

[emf]

Sounds like you made some great progress. I've still been playing around with the basics. I finally got a few sets of sample data captured last night and figured out how to get the simulator to use it. At least now I can work on the algorithms in a comfy chair.

I've got to say I'm pretty surprised with what the AVRs are capable of. When I did a few quick tests of the 8-bit PICs, it didn't seem like it would be possible to get anything like real time updates without doing a lot of optimization (fixed point & table lookups). From what I remember, the standard library trig functions took about 4000 instructions (400usec), floating point division took a little less than 1000. I never tried a full filter, but it just seemed like it would never work.

Anyway, I should have something up & running by the time you get back from vacation. Should be interesting to see what we can come up with.

[mpthompson]

My AVR Studio project which interfaces an AVR ATmega168 with the SparkFun IMU 5 can be found at the following URL:

http://home.comcast.net/~michael.p.thom ... _IMU_1.zip

The README.TXT file describes how to connect the IMU 5 to the AVR STK500 development system for testing. The Kalman filter is used to determine pitch in degrees along the X and Y axis of the IMU at a 10 Hz rate. The pitch values are output on the spare RS232 port at 57.6K baud.

I'm fairly pleased with how my Kalman filter code seems to be working so far, but I would really appreciate input from other people who would have more insight into the Kalman algorithm. In particular, how to go about tuning the values used in the measurement and process noise covariance matrices.

I hope this code will be useful to others and I would be happy to discuss suggestions on how to improve it.

-Mike

[emf]

Thanks for the code. It's nice to start with something simple and readable for a change, instead of diving straight into the 7 state quaternion-based filters. It feels like I have a chance of understanding this one.

I haven't tried out your code yet, but one thing I noticed is that you're keeping the two axes completely separate and I think that means you're feeding bad gyro data into the filter. I think I'm right on this, but I could be misunderstanding something.

Let's say the IMU is flat. First roll 60 degrees. The gyro will tell you that you rolled 60 degrees, and the accelerometer will agree. Now, pitch up (from the IMU's point of view) 60 degrees from there. Your gyro tells you that you pitched up 60 degrees, but the accelerometer will tell you that you really only pitched up 30 degrees because a good hunk of that was converted into yaw.

The error is probably pretty small for small bank angles, so you might not notice it unless you do a few abrupt moves without giving the accelerometer a chance to stabilize things.

[mpthompson]

Hmmm. I understand the issue you raised and it may explain some weird data I have seen on the output. I'll do some testing and see if I can reproduce what you describe. For now, my goal is to balance a biped robot so I probably mostly be dealing with small bank angles (unless it falls over ), but I certainly want a correct implementation that will handle large bank angles as well.

My initial thoughts are that the issue can be handled outside the Kalman filter by adjusting the gyro rates based on the estimated attitude around the X and Y axis -- essentially factoring out the yaw component. Hopefully this can be done without a Z axis on the gyro or adding too much overhead for the required trig functions.

-Mike

[emf]

My initial thoughts are that the issue can be handled outside the Kalman filter by adjusting the gyro rates based on the estimated attitude around the X and Y axis -- essentially factoring out the yaw component. Hopefully this can be done without a Z axis on the gyro or adding too much overhead for the required trig functions.

I think that's basically correct. My guess is a correction like this:

Code: Select all

gyro_x += gyro_y * sin(x_kalman_state.x_00 / 57.2957795) * tan(y_kalman_state.x_00 / 57.2957795);
gyro_y *= cos(x_kalman_state.x_00 / 57.2957795);

in the main loop would do the trick. If you plan on doing a lot of falling while twisting, the Z gyro might be a good investment, but if you can stay mostly level it's probably not a big deal.

If you want to be pedantic, it might be best to apply the gyro bias before the correction, but this should be a pretty small difference.

Would there be any benefit to combining the two single-axis filters into one four-state filter that handled both axes? Does it allow the filter to establish a relationship between the gyro biases for each axis?

[mpthompson]

Would there be any benefit to combining the two single-axis filters into one four-state filter that handled both axes? Does it allow the filter to establish a relationship between the gyro biases for each axis?

That's a good question. Could it be done without increasing the size of the matrices? We are on the ragged edge right now of what the AVR can do with floating point values. Perhaps a move to fixed point math would allow a filter with more states.

BTW, I've been testing things a bit. When the pitch is at near 90 degrees, the roll value becomes extremely senstive to small angular changes. The slightest change will cause the roll to undergo dramatic changes such as +/- 90 degrees. Likewise when roll is near 90 degrees, the pitch value then becomes extremely sensitive. I believe the source of the instability is the z-acceleration value approaches zero which is the second value in the atan2() function. With pitch at 90 degrees the x acceleration goes to about 1 g and both y and z acceleration go to near zero making roll meaningless. In such a situation it seems the roll value should stop using accel_z and switch to accel_x as the second value to the atan2() function.

I'll try your suggested changes and see if that helps things a bit.

-Mike

[emf]

That's a good question. Could it be done without increasing the size of the matrices? We are on the ragged edge right now of what the AVR can do with floating point values. Perhaps a move to fixed point math would allow a filter with more states.

I've got a hunch it takes more math. Maybe I'll try converting it and see how bad it is.

BTW, I've been testing things a bit. When the pitch is at near 90 degrees, the roll value becomes extremely senstive to small angular changes.
(snip)

This sounds like it could be related to something I noticed a few threads back that kind of surprised me. When I ran the numbers, a 10 bit ADC running at 3.3V could take accelerometer readings that are slightly better than 0.01g. On the good side, the difference between asin(0) and asin(0.01) is 0.57 degrees. On the bad side, the difference between asin(0.99) and asin(1) is 8.1 degrees. I knew there was going to be a difference in sensitivity, but I was surprised at how bad it gets. I'll have to see if atan2 behaves any better tomorrow when I'm thinking straight.

If it doesn't, it certainly seems like we should be switching which axis we're looking at after we cross some threshold so that the worst case would be around the 45 degree marks. But smoothly switching between axes sounded like it would be tricky to get right, so I promptly forgot about it.

Anyway, I'm not sure how relevant this is to what you were noticing, but I'll give it some thought.

[mpthompson]

An updated project file can be found here:

http://home.comcast.net/~michael.p.thom ... _IMU_2.zip

This new file contains:

1. A bug fix where I was passing in the measured gyro delta between samples rather than the measured gyro rate into the Kalman filter. This resulted in effectively multiplying the gyro rate by the sample period twice.

2. Changed the math to use radians rather than degrees for easier integration with trig functions. The output to serial port is converted to degrees for display.

3. Modified the initialization code to remove all hard coded constants from the kalman.c file to the init function.

4. Clean up of variable names in main.c.

I did attempt to implement the following:

Code: Select all

gyro_x += gyro_y * sin(x_kalman_state.x_00 / 57.2957795) * tan(y_kalman_state.x_00 / 57.2957795);
gyro_y *= cos(x_kalman_state.x_00 / 57.2957795);

but ran into issues when pitch or roll went near 90 degrees. I'll need to examine this a bit after some sleep. It's commented out in the new project code for now.

If it doesn't, it certainly seems like we should be switching which axis we're looking at after we cross some threshold so that the worst case would be around the 45 degree marks.

Exactly my thoughts as well.

-Mike

[emf]

An updated project file can be found here:

Coffee hasn't fully kicked in yet, but something keeps catching my eye. When you rotate the IMU around its X axis (I've been calling this "rolling"), you'll see the change in x_rate, and it will cause a difference in the ratio of y_accel and z_accel. Likewise, rotating about the Y axis ("pitching") shows up on y_rate and x_accel and z_accel. From the code, it looks like you're using the wrong axes to calculate the roll & pitch from the accelerometer. Is this fixed in wiring or in a variable swap I'm missing? This bug(?) was in the original version of the project too.

[mpthompson]

Hmmm. I think I see what you mean. Using X, Y and Z axis as defined by the markings on the IMU 5 PCB, I've been calling "pitch" rotation around the Y axis and "roll" rotation around the X axis. "pitch" is this measured directly by the X and Z accelerometer values (through the atan2() function) and "roll" is measured directly by the Y and Z accelerometer values. Therefore, "pitch rate" should be measured around the gyro Y axis and "roll rate" should be measured around the gyro X axis. My code appears to have had the gyro values swapped.

The funning thing is that when I correct the gyro rates, rotate the IMU the output looks very similar to what ti looked like before. This would seem to indicate that the gyro rate information has very little impact on the estimated value. Likewise, the output seems overly sensitive to accelerations alongs the X and Y axis and we want to gyro to help stabailize this output. I'll need to see if changing the process noise covariance values (Sw_00 and Sw_11) can minimize such sensitivity.

Hopefully there aren't bigger issues with the code .

-Mike

[emf]

The funning thing is that when I correct the gyro rates, rotate the IMU the output looks very similar to what ti looked like before. This would seem to indicate that the gyro rate information has very little impact on the estimated value. Likewise, the output seems overly sensitive to accelerations alongs the X and Y axis and we want to gyro to help stabailize this output. I'll need to see if changing the process noise covariance values (Sw_00 and Sw_11) can minimize such sensitivity.

This stuff is going to be hard to test since it'll always have to be done on the hardware. It would be *really* nice to have sets of test data that also included physical measurements of the angles so we could analyze how much error we're getting. Something tells me I'm going to spend a few hours driving figure eights in empty parking lots trying to get some tricky data...

Regarding atan2, it seems to give pretty good data. If the test program I wrote is correct, with a 0.01g resolution we shouldn't see an error of more than .6 degrees. So the atan2 function probably isn't the source of the bad behavior at high pitch/bank angles. I'll probably look at that this weekend when I should have time to get the code ported over to my hardware.