MAV-blog

Stuff related to MAV's (and UAV's) from a hobbyist's point of view. Info

Kalman demo application

A lot of people saw I purchased a 5DOF module from SparkFun and even made a test-PCB for it. Obviously, they asked me if I was willing to share my code. Well alright, here you have it.

A simple embedded kalman filter, nicely documented. Also featuring, for the interested:

  • Using the ADC hardware module in a dsPIC30
  • Using the UART module in a dsPIC
  • Using the timer module in a dsPIC
  • How to organize your code! I’ve seen some hideous projects on the web.

The code is written using Microchip’s C30 compiler . The non-optimizing version is free!

I wrote a small app to visualize the output better:

And last but not least: downloads!

Update:
There is a bug in Microchip’s latest libs. You need to add: #define UART_ALTRX_ALTTX 0xFFE7 /*Communication through ALT pins*/
to uart.h or to the code that references it.

5 December 2007, 21:47 | Link |

  1. Great!!! you rules!!


    — Jordi, 6 December 2007, 01:08 | #


  2. Do you have any plans to do a full blown autopilot using a dsPIC? If so, I would not mind helping out, I’ve been using dsPICs for some time now and think they are really nice. Cool demo, nice comments on the code by the way. Keep it up. You Rock!!


    — Mariano, 12 December 2007, 06:21 | #


  3. I’m actually working on a “full blown” autopilot system. But it will take some time before I make it public. First I want to create a “reference” hardware module. As long as this part isn’t finalized, there’s no use in letting different people work on it, each with a different version of the hardware.
    I hope the “final version 1” will be with a dsPIC33 (or maybe PIC32?) Right now I’m still using the dsPIC30, which still has the following advantages: 1) 5V and 2) EEPROM memory for storage.
    Any other views on this?


    — Tom, 14 December 2007, 14:26 | #


  4. hello!i am very glad to hear from you.i and my partner are doing something about the UAV.
    i am puzzled recent. could you tell me how to set the log data format of the MP2028. i do not know how to receive the data and how to set the data format received. thx!


    — Suny, 16 December 2007, 14:39 | #


  5. Suny,
    I propose you contact the Micropilot support people, they are very helpful :-)


    — Tom, 20 December 2007, 16:36 | #


  6. Fantastic work Tom. I’m just wondering about a couple of this relating to the video demo above. Firstly, is the code you published the one you’re using in the demo. In particular, I’m wondering if you are using the same initialization numbers for the covariance and noise in the filter. In my implementation, I am seeing very slow convergence and am wondering how you are getting such good response. Thanks.


    — Daniel Wee, 21 December 2007, 07:48 | #


  7. Hello Daniel,

    The code I used in the video is the same as the one I published. Yes, if the kalman filtered angle and the accelerometer angle differ a lot, the convergence of the kalman filter towards the accelerometer value is very slow indeed, but that’s a good thing! I think that your gyro’s (initial) neutral values are a bit different from mine… you could adjust them in gyro.c
    Normally, if you let it convergence a while until accelerometer and kalman angles are the same, it should be as responsive as mine.


    — Tom, 21 December 2007, 08:50 | #


  8. Hello Tom,
    Nice work and very informative web site.

    Can you provide me with the code (and bin) of the tool you wrote to visualize the output ?

    thanks in advance,

    regards,
    arno


    — arno, 25 December 2007, 22:10 | #


  9. Hello Tom, Sorry for the late response but been out ‘cause of the holidays.

    I think using a dsPIC30 is a great idea, aside from the advantages you mention, there is also the DIP availability. Besides going from a 30 to a 33 should be very easy. Regarding a PIC32 I’ve read mixed reports (some great, some not so great) . I personally think the platform and variety will get really good in about a year time-frame (there is still no CAN capability for instance and 2 serial ports for a 32-bit is quite limited). I think your choice for a dsPIC is the best choice on the Microchip family. Again I wouldn’t mind helping out in any way. Also, providing a list of capabilities you want to include in your autopilot would be great to get feedback very early in your design process. You Rock!!
    — Mariano, 31 December 2007, 07:11 | #


  10. Hi Tom

    To add to the debate, I am thinking of basing an autopilot design on the PIC32 starter kit board. While at 53mm square, it may be a little wide for some people, it is still smaller than the Pentax A30 camera that I have to fit in my plane anyway. It does have the advantages of a ready made USB programming interface with a few led’s and switches. More to the point, all the complexity of SMT board design and assembly is removed.

    I then plan to build a daughter card to plug into the the PIC32 starter board. This with have connectors for the Sparkfun IMU5, the SCP1000 pressure sensor break-out board and the EB85A integrated GPS and Antenna unit. The daughter card will also have the servo connector pins and power supply regulators. Overall size will be about 53mm * 90mm with a height of about 25mm. As far as I can see, with the exception of the SMT connector to mate with the PIC32 card, the remaining connectors and glue components can be normal through hole stuff, making the board easier to design and assemble.

    Any comments would be welcome BEFORE I start doing too much work. I see Mariano has metioned some negatives about the PIC32. Any references would be a help.

    PJG


    — PaulJG, 6 January 2008, 15:33 | #


  11. Hi, nice demo, very inspiring!

    I’m a very novice when it comes to Kalman filtering, but can the example application measure the yaw rotation, without any modifications?

    Does the mounting of the accelerometer due to the gyro always follow the same pattern, the accelerometer’s ”measuring directions” perpendicular to the gyro axle? Or does it vary depending on witch rotation is measured?

    — Lilja


    — Lilja, 20 January 2008, 13:08 | #


  12. @Lilja,
    No, yaw rotation can’t be measured because:
    1) we don’t have a yaw gyro
    2) we would need a magnetic compass to compensate the drift in the yaw gyro

    You can choose how to position your accelerometers, as long as there as a way to mathematically transform them to the gyro positions.


    — Tom, 20 January 2008, 13:53 | #


  13. @Tom

    Thx for the fast response.

    Ok, even if pointing your device upwards (so the gyro gets in position as a yaw gyro), the accelerometer will not give needed measurements and therefore the need of the compass?


    — Lilja, 20 January 2008, 23:13 | #


  14. Since I have figured out the answer on my previous question… Following one appeared.

    If mounting two accelerometers with a known (constant) distance from each others, the acceleration of the rotation could be calculated (even if moving in one direction while rotating). Could the rotational acceleration be combined with the gyro output in a Kalman filter? Or is there any other way to combine them to estimate rotation speed and further the actual heading?

    Sorry if this is going a little off topic.


    — Lilja, 21 January 2008, 13:46 | #


  15. Tom,

    I see that your code is only 1 dimensional and it looks like it works with the roll axis. How would you go about incorporating the second pitch axis into the Kalman Filter? Thanks.


    — Tim Hoelle, 17 April 2008, 19:59 | #


  16. Lilja,

    You would not need two accelerometers. You could do it with just a single accelerometer if you know the distance from the center of rotation.


    — Nathan, 21 April 2008, 22:36 | #


  17. Tim,
    In a simple implementation, pitch and roll can be treated independendly of each other. Only when transforming the roll-gyro’s values to the world coordinate system , they will have a dependency. I implemented this as a precalculation.


    — Tom, 22 April 2008, 19:53 | #


  18. Do you know the update rate of the filter in your demo? Any idea what minimum update rate is required to get good results on roll & pitch with the IDG-300?

    Thanks,
    Jon


    — Jon, 26 June 2008, 03:30 | #


  19. Hi Tom, it is amazing that exists someone that share such knowledge. Thanks a lot. I’m new in INERTIAL SYSTEM and with your work I did a big step. I adjust your algoritm on a PIC32 and it works very fast. Thanks again!

    Fabian


    — Fabian Rezende, 2 August 2008, 19:45 | #


  20. Hi Tom, could you post your VB demo app code? I’d like to expand on it to include pitch information. Your Kalman filter implementation is the cleanest I’ve seen, great job!


    — Bob, 15 August 2008, 11:58 | #


  21. Tom,

    Your Kalman implementation looks to be identical to tilt.c…so…

    How do you solve lateral acceleration? In an aircraft z may remain constant (coordinated turn) and y or x may change proportionally when skidding (lateral acceleration) or forward acceleration or decleration. 5DOF+K-filter interprets lateral acceleration as pitch up/down (climbing, diving) or skidding )(roll in opposite direction of skid).

    Paul Mace


    — Paul Mace, 4 September 2008, 00:16 | #


  22. @Paul

    It’s not exactly the same as tilt.c, but obviously there isn’t much room for variation.

    I substract lateral accelerations by modeling them using GPS data. Simplifications such as no sideslip, etc make the model workable.


    — Tom, 4 September 2008, 18:51 | #


  23. Tom,

    is the scaling needed or not,
    the difference between
    accelero_roll_radians and PredictAccG_roll

    or why do you use PredictAccG_roll in stead of the accelero_roll_radians function


    — Tim, 20 September 2008, 17:57 | #


  24. Just an FYI for those who may be using this stuff in their own applications: Be sure the data being passed to the filter is correct.

    I took Tom’s code and ran it ‘as is’ with my hardware. It worked, but it seemed slow to respond. My gyro values were off by a factor of 2.

    I discovered this by doing my own gyro update, like this:

    MyGyroValue += GyroValue * DeltaTime;

    I then compared MyGyroValue to the Kalman output and saw that my numbers were way off, and the ‘settling time’ from the Kalman filter was the accelerometer correcting my bad numbers, though quite slowly.

    Now that I’ve corrected the problem, the output from the Kalman filter is MUCH more responsive.

    Jason


    — Jason Dorie, 7 October 2008, 06:10 | #


  25. Hi Tom,

    I was wondering what dsPIC30 you used for this? Do you apply any analog or digital filtering to the data? What is the sample rate for each channel?


    — Jeff, 2 November 2008, 02:54 | #


  26. Hi Jeff,
    you can use any dsPIC. I use only digital filtering at about 100Hz sampling rate.


    — Tom, 15 November 2008, 21:25 | #


  27. Awesome demo. My undergrad senior project is implementing a kalman filter for heli flight. One question, we’re using the IMU 6dof v4 that uses the same gyro but a different accelerometer(MMA7260). Would we have to do any major changes to use these accel adc inputs with your code?


    — John, 30 November 2008, 02:14 | #


  28. Can this be used for Robot Navigation in a 2D environment, meaning that the sensor would give me the rotation about its own axis (Z)-Relative Frame ? thanks


    — Diego Martinez, 4 December 2008, 22:13 | #


  29. Hi Tom,

    Thanks for sharing the codes. That is really a great demonstration. I have 2 questions:-

    1. How are Q_angle, Q_gyro and R_angle determined in an particular application? are they sensor dependent and need experiments to determine? If so ,any clue how to do this.

    2. Could you share your code of visualizing the output(in VB?)? so that I can monitor the response on PC as what you shown.

    -Thanks.


    — wanghar, 14 December 2008, 08:56 | #


  30. Hi Wanghar,

    Q and R should be determined with experiments. They are sensor depended. I usually change the values with a factor 10 until they look good.

    The artificial horizon code can be found here:
    http://tom.pycke.be/mav/100/artificial-horizon


    — Tom, 14 December 2008, 11:08 | #


  31. Hey Tom,

    Would it be possible to take a look at your full Kalman code? I am working on an INS with this IMU module, and I am curious to see how you implemented pitch as well. From those pieces of data, I can construct yaw (based on some assumptions), and get a full orientation estimate that updates faster than GPS.

    Thanks!


    — Spencer, 20 December 2008, 00:38 | #


  32. Hi Tom,
    is it possible if you can include the part list of this project, and what programmer did you use to transfer the Hex file to the micro controller or did you use the In Circuit Serial Programming, thanks for all your help


    — Diego Martinez, 5 January 2009, 18:35 | #


  33. Tom,

    I am a senior EE major at working on a tracking project incorporating accel, magnetometer and gyro, and can get rough distance and heading data, yet want to incorporate a Kalman filter to clean it up a bit. I am having trouble implementing the filter, from beginning to end in software. I have read and re-read SO much documentation but am still stuck! What advise or help can you offer? Instead of code, could you give a step-through of requirements and functions?


    — Kosta, 10 February 2009, 07:17 | #


  34. Kosta, I would propose to read this thesis:
    http://www.essays.se/essay/3b4b050afe/


    — Tom, 11 February 2009, 12:52 | #


  35. Fantastic piece of code.
    I have a question…
    I have a 3DOF accelerometers and 3DOF magnetometer.
    I would like to replace your “roll filter” by a “yaw filter” using the magnetometer instead a gyro…
    My accelero give me force in “G” (with 1G for gravity (z axis))
    My magnetometer give me angle in rad.
    How should I modify your code ?
    Thanks a lot.


    — psykokwak, 12 February 2009, 19:25 | #


  36. Tom,
    I have a question, I have adapted your Kalman filter code on a PIC32. How can I tell if the filter is working correctly. I am using the IDG-300 gyroscope and the ADXL-203 accelerometer. Do you recommend getting the 5 DOF sparkfun, for the kalman filter code to work. I using the filter to fuse both sensors, to balance a robot.

    Thanks in advance.


    — John C., 13 February 2009, 23:46 | #


  37. Tom,

    Whats the difference between ars.c and ars_micropilot.c in the source? Thanks


    — InterMag, 6 April 2009, 06:47 | #


  38. Hello Tom,

    Your code is basically a robust 1D (pitch or roll as you prefer) tilt sensor and at this point the Kalman filter is simple and fast enough. I have a 6dof imu and need a 2D (pitch and roll) info. This gets complicated because the 3 gyro axis are coupled and one can not handle them separately from each other. Going to full 6dof kalman gets complicated. Is there a way to have a robust pitch-roll sensor using a combination of two 1D sensors (or something else of this kind).

    Thanks Tom.


    — tadej, 14 April 2009, 11:30 | #


  39. Hi.

    I’m trying to balance my bipedal robot with one inclinometer. Is it possible to do a software based filter for it? I’m not having any luck. My idea was to use last measured data to correct the new data, but it’s not that simple to write a code that really works that way.


    — Michael, 2 June 2009, 13:29 | #


  40. Thank you very much for sharing this!


    — Alex, 18 August 2009, 12:06 | #


  41. Hi Tom, if you pay attention to the Kalman gain, always trying to adjust the prediction to be as the second sensor in this case is the accelerometer, if you increase the speed of processing the algorithm, you will end up with the prediction equal to the accelerometer in others words, slower iteration process will slowing down the chase and will miss the rapid change in the accelerometer, that leads to pass the slow changes, means you have software low pass filter only, to prove this I took pic32 running kalman algorithm against pic18 with moving average , the pic32 after tuning gave same result as the moving filter technique , all you see in you demo is the low pass filter only, simply take each sensor as person that tells truth some times (you can not conclude truth from two unreliable sources) another point is the low pass filter doesn’t have glitch when you jump from 360 to 0 degree in your demo you don’t go there, rotate the sensor beyond 360 and watch the measurement how it goes crazy, thanks for the info.


    — rf_clock, 19 August 2009, 00:08 | #


  42. Hi: I am very glad to study your code,now i have a question,does your code can detect human gesture? Because i am a new man, i use Atmel microcontroler-Atmega128L microcontroler.


    — Mcdull, 25 August 2009, 03:36 | #


  43. Good afternoon. O, it is excellent to have a giant’s strength; but it is tyrannous to use it like a giant.
    I am from Bahrain and learning to read in English, give true I wrote the following sentence: “Why Handmade soap ph?Because in tattooing with its great rise, lush has away intended to flourish out its conversion of shape wealth leather, handmade soap ph.”

    Thanks :-D. Best handmade soaps.


    — Best handmade soaps, 28 August 2009, 07:33 | #


  44. Hello,
    I have reviewed you code and have a question: as far as I understand you code you do not read the accel and gyro data at the same time?

    Regards


    — Greg, 31 August 2009, 21:35 | #


  45. Hi Tom,
    my interest is to build a DIY Segway; because I am a beginners, can I think to buy your KalmanTestBoard fully programmed?
    thanks, Beppe


    — Beppe, 26 September 2009, 07:57 | #


  46. Hi Beppe,

    You can buy a newer version. Contact me if you need more info.


    — Tom, 8 October 2009, 07:17 | #


  47. tom,i need kalman filter for arduino. for using imu 5 dof or imu 6 dof. please help me


    — myhalici, 15 October 2009, 07:27 | #


  48. tom I have been reading your code, and the Kalman filter theory and I haven’t yet encounter how you made the fusion of the two sensors,but I can see that you are taken the accelerometer value as the truth value. could you explain me how the Kalman gain weights the measurements of the two sensors?


    — chelitog, 28 January 2010, 23:22 | #


Name
E-Mail
http://
Message
  Textile Help

|