Kalman Filtering IMU Data

[Chris]

Hey everyone,

This website is awesome! I wanted to start this topic to discuss and improve Kalman filters for IMU data.

I downloaded the "KalmanTestBoardV1.zip" from Tom's website at "http://tom.pycke.be/mav/92/kalman-demo-application", which I realize is from 2007, but it was the only code I found.

A couple of things I noticed:
1) In the ars_predict function Q is multiplied by dt, but I don't think it should be multiplied by anything. It's not in the comments above the function and I looked it up in some other resources.

Code: Select all

//Comments
 * P = F P transpose(F) + Q
 *
 *   = [ 1 -dt, 0 1 ] * P * [ 1 0, -dt 1 ] + Q

//Code
   filterdata->P_00 +=  - dt * (filterdata->P_10 + filterdata->P_01) + filterdata->Q_angle * dt;
   filterdata->P_01 +=  - dt * filterdata->P_11;
   filterdata->P_10 +=  - dt * filterdata->P_11;
   filterdata->P_11 +=  + filterdata->Q_gyro * dt;


2)In the ars_update function P_00 and P_01 are recalculated and half the lines use the new values. So in the code below, lines 1 and 2 use the old P_00 and P_01 and lines 3 and 4 use the new value. I simulated the results and it appear to have a negligible effect, but thought I'd mention it.

Code: Select all

   filterdata->P_00 -= K_0 * filterdata->P_00; 
   filterdata->P_01 -= K_0 * filterdata->P_01;
   filterdata->P_10 -= K_1 * filterdata->P_00;
   filterdata->P_11 -= K_1 * filterdata->P_01;


[asifjahmed]

Hi Chris -

I am also very interested in Kalman Filtering, and will be using it on my gluonpilot board, hopefully in the near future.

What are you using for simulation?

I currently have a quadrocopter (which I plan to port over to the gluonpilot) which is based on arduino.

I have the same accelerometer (ADXL335) and when trying to use it, I get very very noisy readings when my motors are running. Do you think this is normal? Will a kalman filter be able to sort through all of that noise?

Thanks,

-Jamie

[Tom]

@Chris
(all this out of my head as far as I remember it from 2 years ago, excuse me for any mistakes )
1) The example you downloaded from my website uses a standard (lineair models) kalman filter.
You can also use an extended Kalman filter, which gives you a slightly different result: including the dt term in that equation. It my mind, it made sense to make the "constant" dependent on the dt-term as the extended kalman filter would predict, so I included it as an experiment.
Indeed, no remarkable difference in performance

2) I don't really understand what you changed here, but always be carefull when doing these kinda changes. It can affect the "stability" of your filter.


@Jamie
You should really explain your setup a bit more. Is your frame vibrating a lot? Does the power supply to your module come from the motor's ESC? Can you put the accelerometer's output in an excel (with meaningfull axis)? Did you try averaging them? At what sample rate are you sampling the accelerometer?
Usually the problem is vibration... Averaging it might help, the kalman filter will also help.

Tom

[Chris]

@Jamie

I've been simulating using MPLAB SIM and plotting data in Excel. In MPLAB SIM you can step through your code with a "stimulus" file of ADC conversions. I generated the stimulus file by just running the chip and storing actual ADC data. I also had the chip output a lot of data to a text file for sensor readings , accel arctan angels, Kalman angles, and control outputs and had Excel plot it. You could probably do something similar in real time with Visual Basic, but I've never used that program.

Check out this project for a good example of a quadcopter with a Kalman filter. He talks about some issues with the accelerometers also:
http://vrhome.net/vassilis/category/qua ... =1680x1050

@Tom

Have you posted your new Kalman code? I would be really interested to see it.

So the code I pasted up there was your original copied from your ars.c, the modified code I changed is this:

Code: Select all

   filterdata->P_00 += ( -dt * (filterdata->P_10 + filterdata->P_01)) + filterdata->Q_angle;
   filterdata->P_01 += ( -dt * filterdata->P_11 );
   filterdata->P_10 += ( -dt * filterdata->P_11 );
   filterdata->P_11 += filterdata->Q_gyro;


- and -

Code: Select all

/*Must calculate P_10 and P_11 first if they are to use the same vavlues of P_00 and P_01 that P_00 and P_01 use in their calculation.  Hopefully that makes sense...  */
   filterdata->P_10 -= K_1 * filterdata->P_00;
   filterdata->P_11 -= K_1 * filterdata->P_01;
   filterdata->P_00 -= K_0 * filterdata->P_00;
   filterdata->P_01 -= K_0 * filterdata->P_01;


[Byron Blue]

I tried the second change and found that it caused a disaster (readings of ~1000 degrees off). First change made little if any difference.

[Chris]

Byron, thanks for testing the code. I hope more people are trying it. I found the difference with the first change is small, too. Since the Q's Tom used originally (and they work great for me) are 0.0001 and 0.0003 and the covariances (P_xx) end up 0.0something, the Q's already make only a small difference at each step. Multiplying them by a small dt makes them have even less effect. If you were to use larger Q's it would make more of a difference.
The second change should be small also. This change is only for the "update" function just to be clear. Since you're just changing between the predicted or updated P_00 and P_01 values (which are very close), the difference should be very small. I'll have to double check after break, but I think this exact code is on my setup and runs perfect. Bottom line, it's a really nit-picky change and you'll be fine without it. Have you made any observations or improvements to the code?

[Byron Blue]

Chris
The majority of changes I've made basically amount to running averages to reduce/remove the effects of vibration, external calibration and scaling.
I'm using a PIC24 so the code had to undergo some revisions just to get it up and running. It is behaving somewhat although I am still working on the scaling...

[Goldfinch]

Tom, thank You very much for Kalman filter example. I'm learning it and it will be helpful for me. I want to make Simulink model and test it with real IMU data (ADIS 16405). I have similar task to correct Euler's angles of small UAV with accelerometer data but my first steps are not consolatory. I've read many theoretical articles but with no implementation explanations. I have some questions, maybe they have simple solution. Are there any limitation for Kalman filter correction in coordinated turn with constant altitude? In this situation roll measurement by accelerometer will fail. So, maybe it is possible to make variable weight coefficients depending on flight mode (straight flight with constant course, speed and altitude when body accelerations are small with compare to G)? Did You test complementary filters (combining LPF with HPF) for angle correction?

[Tom]

Hi Goldfinch,

The non-gravity accelerations during a turn can be accounted for in different ways:
- By modeling the centripetal force: function of speed and turning rate. Check your physics/mechanics book
- Modeling speed along z-axis (less common): height difference, pitot tube.
- Modeling accelerations in speed along x-axis: pitot tube (less common)
- Modeling the influence of the rotation of the earth (even less common)

Usually only the centripetal forces are modeled, which is OK for your average UAV

The complementary filter can be used, mostly when your gyros are stable enough (should be OK with your sensors). A Kalman filter will however work better

Tom

[Goldfinch]

I've asked about coordinated turn (with centripetal force occurance) because for me it is not obvious that Kalman filter will track roll angle correctly. Because coefficients are constant and doesn't change in dependence of flight mode. So update part of Kalman filter will measure zero roll. But I had no much experience and made some test flights without angle correction (for 5 minutes angles walk for 5-10 degrees), it is only my opinion. I think that on-board modeling for angle estimation is not good method. I prefer to work with real sensor data. I think that both complementary filter and Kalman filter need in stable gyros! I've found one method of angle correction, but I can't understand its mathematics. It is in Merhav's "Aerospace sensor systems and applications" (http://books.google.com/books?id=GSyNye99dz0C&lpg=PR7&ots=nE3qbhLoYn&dq=merhav%20gyro%20aiding&lr=&pg=PP1#v=onepage&q=&f=false) Page 415.