Kalman Filter (Working) Small bug



  • Jack CrenshawJack Crenshaw Posts: 46
    edited 2009-01-25 - 20:15:34
    A few thoughts on balance bots, Kalman filters, et al. I don't claim to be an uberexpert, but I have worked with and around nav systems for years.

    1) In the real world, we always calibrate the IMU -- sometimes for days -- before it's even installed in the system. This involves putting it on a tilt table and rotating it at various rates (for accels, just make sure output is 1g when each axis is down).

    2) When the completed system powers up, it needs some time (minutes) to calibrate and align itself. Tp dp this, nothing can move. The KF needs to know that nothing is moving, so that any biases are, indeed, biases. You can have the bot pause periodically to get another align, if necessary.

    3) With a PID or PD controller, the 'D' part is essential. The bot is a dynamic system, meaning that its equations of motion are second order. Without the D feedback, the control system will oscillate forever.

    4) But, as others have said, don't use a divided-difference to get the D term. You don't need to, since that's what the gyros put out.

    5) NAV systems need some kind of "ground truth" info to keep them from wandering off. JWood is using an encoder on the wheels -- good idea. Both the position (encoder counts) and speed (tach) are useful. GPS is great, of course, but only when used to trim the KF. Not so good, alone.

    6) About balancing bots: From the broomstick model, it's easy to see that one needs a hierarchical control system. To go forward, say, the bot first needs to move backward, enough to allow the motors to accelerate.

    I'm assuming that you guys all know all this stuff already; probably much more experienced than I. So please don't be offended if what I'm telling you is old news. Just thought to give my $0.02.


  • EyalMEyalM Posts: 2
    edited 2009-02-02 - 14:47:27
    I'm using a 6DOF board from SparkFun for stabilizing a helicopter project. I wanted the 6DOF to be able and get also the YAW stabilized.
    I'm using JWood's Kalman filter and modified IMU code adapted to the 6DOF board.
    Does anyone know what change is needed to the code in order to get a YAW stabilized angle? For the moment I have only pitch and roll angles from the original IMU code and I figured I will need a third Kalman filter for the YAW? How is it done?
  • JoeBoticsJoeBotics Posts: 17
    edited 2009-02-02 - 16:00:44
    Hey Jay, I don't believe you can fit the slimmed down Float32 into the main IMU, the version that I made is a simplified Float32Full that is missing a bunch of the functions, and I only left the important ones related to the IMU calculations, the simplified Float32Full barely fits in one cog, but I can give you an idea if you are running out of cogs, use a parallel interface ADC and just add the read commands directly in your code, you will save one cog by eliminating the need for the MCP308, one ADC IC option would be MAX198, it is a 6 channel, 12bit, parallel interface, it comes in DIP package and has the option for external voltage reference... I hope this helps... smile.gif
  • AnubisbotAnubisbot Posts: 112
    edited 2009-02-02 - 18:04:28
    since i work on a simular project a tricopter with contra rotating props, so i have a question, would it be wise to use a float co processor what also could do some calculations or maybe even house the kalman filter ??
    i was thinking of using this one..

    Floating Point Co-Processor uM-FPU v3.1 SOIC

    it seems to be nice to relieve the prop from the floating point math and the gps phrasing, and also it gives some additional memory.
    would that be a good editon for all the quad flyer ??

    Best regards Anubisbot
  • Jay KickliterJay Kickliter Posts: 446
    edited 2009-02-02 - 20:04:15
    Anubisot, if you are going to do that, it might be easier just to use a second prop to do all the preprocessing for you. From what I understand those chips are just uC's with some floating point libraries. Somebody correct me if I'm wrong.

    JoeBotics, you're right. I've been learning assembly, and was not able to get it all on one cog. But, using just the minimum math functions (not the whole float32 object), and converting the KF/IMU code to assembly, I've just about slimmed it down to have 1 axis running in a cog, and another cog running the ADC code, with almost enough room to spare for a second axis (I need to learn more about optimization).
  • ianwianw Posts: 32
    edited 2009-02-03 - 21:09:08

    I assume you're aware of the Kalman filter and INU objects created by I.K
  • ianwianw Posts: 32
    edited 2009-02-05 - 21:17:50
    I found two threads from ~2 months ago about nmea/gps on the uM-FPU, I'm listing them for reference:

    The threads were not conclusive as to whether anyone stuck with the FPU for gps/nmea though
  • shanghai_foolshanghai_fool Posts: 149
    edited 2009-02-05 - 23:47:51
    I am 1 of the people using the uFPU with GPS and although I did successfully test inputting GPS directly to the uFPU, I have not yet decided to use it in that configuration. The reason being that the FPU is no longer able to preform other calculations while "waiting" for the GPS string. As I also want to use it for Kalman filter, it would not be available most of the time. I sortof got sidetracked on other devices but expect to get back to this problem shortly.
  • JasonDorieJasonDorie Posts: 1,930
    edited 2009-02-06 - 00:15:38
    There's a modded version of the ObEx IMU code in this thread:
    Near the bottom of that thread is one that I've made an optimization·pass over - it's currently running at 256Hz, and only uses two cogs (ADC and FMath) in addition to the IMU/Kalman cog.· I'm hoping to get the rate above 300Hz, and I think it's possible to reduce it to just one additional cog by merging the ADC and Float32 code into a single cog.· I've got the Float32 cog down to 380 longs, and switching the atan/atan2 code over to use CORDIC should allow the whole works to fit on one, though I haven't run the numbers yet.

  • Jay KickliterJay Kickliter Posts: 446
    edited 2009-02-09 - 00:43:29
    So, I've been working on an entirely PASM IMU/Kalman Filter. It's working well now, with 1700 Hz refresh rate for one axis. The other axis wont be working until I optimize it a bit to free up some longs. I'm pretty much just copying JWoods code, including Jason Dorie's optimizations. My main motivation was to free up another cog, so all the functionality off the IMU can run in two cogs, including the ADC.

    My one question, since I actually don't understand how the kalman filter works, is how state_update and kalman_update work with each other. Are they supposed to oppose each other? For example, when I turn off sate_update, and tilt my board forward, I get a slowing moving positive pitch reading. But when I turn off kalman_update, and turn state_update again, then tilt the board forward, I get a relatively quickly moving negative pitch value. The positive and negative values are arbitrary, but the fact that they appear to oppose each other is what is perplexing me. Is it supposed to do that, or do I have to negate the rate values before sending them to state_update?
  • JasonDorieJasonDorie Posts: 1,930
    edited 2009-02-09 - 19:54:45
    My understanding of this (keeping in mind that I'm not a math guy - if anyone knows better, PLEASE correct me):

    The Kalman filter uses a measurement (the accelerometer) to correct an estimate (the integrated gyro value) and remove bias over time.

    The state update function takes the current gyro value, scales it according to time difference, and adds it to the current estimate of the angle. It also updates the matrix values used by the Kalman filter.

    The Kalman part uses the measurement value along with the matrix results from the state update, to compute an error term, remove that error from the computed (integrated) angle value, and make further updates to the matrices which track the amount of error and bias.

    Short version - I don't think you can use one without the other. If you look at the code, the state update functions add to the p_roll/p_pitch matrix terms, and the Kalman update function subtracts from them, which might explain the behavior you see.

    I have seen versions of this code that do the angle integrate on every call, but only do the Kalman update every few calls. To do that they bypass the matrix term changes in the state update function when only doing the partial execute.

  • Jay KickliterJay Kickliter Posts: 446
    edited 2009-02-21 - 23:40:08
    For peer review, a mostly working PASM Kalman filter/IMU object. The whole thing takes up only 2 cogs, including the MCP3208 code. It is contained in one file. This was initially to make it easier to pass variables between two cogs and make it easier to debug. If you guys thing it is more prudent to split it into two files, I'll do so.

    One cog is running the MCP3208 DAC code, and the other is processing that data and running the IMU/Kalman code, with all the float routines included. It is entirely in PASM, so the Kalman code is running at 2667 Hz for both axis, which is unnecessarily fast, considering the Sparkfun 5DF board is only good for about 300 Hz. The IMU cog is mostly full, but with a little optimization it might be possible to add some PID code.

    This is not ready for release, and I'd appreciate if somebody would be willing to give it a try. I think is is about 90% working. It seems to be taking a long time for the filter to remove bias. There are a few factors that might go into it, but I haven't figured it out yet. It may just be that the scale factor of 0.244 for the gyro rate is too low. It was used in the original code that Jason Wood wrote, so I went with that. Mathematically, it seems correct, but the filter seems to work better at .3 - .4.

    Please, any criticism is welcome, I'd like to make this my first post to the object exchange.


    Please see below post for updated code.

    Post Edited (Jay Kickliter) : 2/24/2009 9:25:01 PM GMT
  • Jay KickliterJay Kickliter Posts: 446
    edited 2009-02-24 - 02:42:27
    Ok, it looks like I have all the bugs worked out. I'm not going to post to the object exchange until I have all the documentation written up. But here it is for anyone interested.

    I have r_angle dialed up to 5.0, as apposed to Jason Wood's original 0.3. You may want to adjust it according to your needs. The lower it is the faster the gyro angle will converge on accelerometer's angle, as I understand it. But lowering also appears to increase noise.

    Post Edited (Jay Kickliter) : 2/24/2009 9:24:18 PM GMT
  • JasonDorieJasonDorie Posts: 1,930
    edited 2009-02-24 - 06:12:17
    I noticed that Jason Wood's original version had that as well, but the number from the code it came from was in radians, and this filter works in degrees, so it makes sense that you'd have to crank it up a bunch. I was going to mention this earlier, but I still had bugs in the optimized version I did, and couldn't verify that it was correct.

    Nice work!
  • Jay KickliterJay Kickliter Posts: 446
    edited 2009-02-25 - 19:29:57
    I see no reason why we should be using more than one cog for IMU processing. Plus, I want to add a third axis. I am no assembley expert, so cleaning up my code might free up a few longs, but not enough to reduce it all to one cog, and defititely not enough to add another axis. But then I ran into this:


    A tilt compensated compass. Besides giving you compass heading, it also gives you pitch and roll. That lets you take the atan2 and some other routines out of the assembly, freeing up room for i2c read/write routines. It only has 1 degree tilt accuracy, but .2 degree repeatability. So it might not work for everybody. Also, you gain heading information which you can filter with another gyro. But, you're still stuck with needing an ADC cog to read the gyros. As was suggested to me above by JoeBotics, that could be eliminated by using a Parallel ADC.

    Whether it would be possible to fit that extra functionality by eliminating the atan/atan2/poly remains to be seen. It would require having several free pins for the Parallel ADC. If anyone is interested I'd love for you to beat me to it. If not, I may have an answer in a few months.
  • bboy8012bboy8012 Posts: 153
    edited 2009-03-08 - 04:24:32
    I was just wondering what modificatins would I have to make, to have the kalman filter meaure pitch, roll, and yaw? Can anyone point me to when I can study this, and get examples? Also does the extended kalman filter cover that? Thanks, appreciate your guys help, and code that I have been able to look and understand somewhat.

  • Jay KickliterJay Kickliter Posts: 446
    edited 2009-03-08 - 23:05:56
    bboy, I'm working on that right now. It does take much at all. I'm doing it in the assembly version I made, but if you want to just modify the original Spin object, you can create another instance of the Kalman filter, feed the other gyro into it, and feed compass data into instead of accelerometer data.
  • bboy8012bboy8012 Posts: 153
    edited 2009-03-09 - 01:00:21
    Thanks Im going to take a look at it. I noticed yours in assembly, but am not familar with assembly at all, so it was kinda intimadtaing. Keep·us posted on yours, I would like to see how it comes out thanks.

  • Jay KickliterJay Kickliter Posts: 446
    edited 2009-03-09 - 18:05:32
    bboy, to tell you the truth, I'm not too familiar with assembly either. At least before starting to rewrite the JWood's code. But after doing a lot of cut and pasting of examples, it started to make sense.
  • MajorStrainMajorStrain Posts: 6
    edited 2009-03-15 - 03:24:17
    Keep up the good work guys.
    I've been keeping an eye on this thread for a while, I'm looking forward to working this baby into an I.C engine quad-copter in the future.

    roll.gif roll.gif roll.gif roll.gif
  • bboy8012bboy8012 Posts: 153
    edited 2009-03-26 - 18:58:36
    Dont know how correct this is, but this is my attempt to add a gyro for yaw into J's Kalman filter. Now that I think I am beginning to graps the kalman theory somewhat, I am going to try and rewrite to not use so many cogs. I think jay is doing the ASM version, still to new to venture into that realm. Let me know any defects.

  • STST Posts: 13
    edited 2009-08-04 - 03:28:17
    OMG - I finally got my robot balancing.· I've been studying and using the object by JWood and then utilized the COG optimized version of IMU posted by JoeBotics.· This freed up some cogs.· Then found the even more cog optimized version by Jay Kickliter and am finally up and balancing as of tonight.· The robot is a bit shaky at the balance point (mind you this was the first time using the IMU) but it works.· Currently I'm only using an untweaked PD controller - will add integral later to help smooth things out.

    Many thanks to everyone that posted and contributed.· It would have certainly taken me much longer had it not been for the 3 major contributors that I identified here.

    Thanks Guys!hop.gif
Sign In or Register to comment.