MAV-blog

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

Kalman filtering of IMU data

Introduction

To many of us, kalman filtering is something like the holy grail. Indeed, it miraculously solves some problems which are otherwise hard to get a hold on. But beware, kalman filtering is not a silver bullet and won’t solve all of your problems!

This article will explain how Kalman filtering works. We’ll use a more practical approach to avoid the boring theory, which is hard to understand anyway. Since most of you will only use it for MAV/UAV applications, I’ll try to make it look more concrete instead of puzzling generalized approach.
Make sure you know from the previous articles how the data from “accelerometers” and “gyroscopes” are used. Some basic knowledge of algebra may also come in handy :-)

Basic operation

Kalman filtering is an iterative filter that requires two things.
First of all, you will need some kind of input (from one or more sources) that you can turn into a prediction of the desired output using only linear calculations. In other words, we will need a lineair model of our problem.
Secondly, you will need another input. This can be the real world value of the predicted one, or something that is a good approximation of it.
Every iteration, the kalman filter will change the variables in our lineair model a bit, so the output of our linear model will be closer to the second input.

Our simple model

Obviously, our two inputs will consist of the gyroscope and accelerometer data. The model using the gyroscope data looks like this:

The first formula represents the general form of a linear model. We need to “fill in” the A en B matrix, and choose a state x. The variable u represents the input. In our case this will be the gyroscope’s data. Remember how we integrate? We just add the NORMALIZED measurements up:

alpha k = alpha k-1 + (_u_ k – bias)

We need to include the time between two measurements (_dt_) because we are dealing with the rate (degrees/s):

alpha k = alpha k-1 + (_u_ k – bias) * dt

We rewrite it:

alpha k = alpha k-1 – bias * dt + u k * dt

Which is what we have in our matrix multiplication. Remark that our bias remains constant! In the tutorial on gyroscopes, we saw that the bias drifts. Well, here comes the kalman-magic: the filter will adjust the bias in each iteration by comparing the result with the accelerometer’s output (our second input)! Great!

Wrapping it all up

Now all we need are the bits and bolts that actually do the magic! These are some formulas using matrix algebra and statistics. No need right now to know the details of it. Here they are:

u = measurement1 Read the value of the last measurement
x = A · x + B · u Update the state x of our model
y = measurement2 Read the value of the second measurement/real value. Here this will be the angle calculated from our accelerometer.
Inn = y – C · x Calculate the difference between the second value and the value predicted by our model. This is called the innovation
s = C · P · C’ + Sz Calculate the covariance
K = A · P · C’ · inv(_s_) Calculate the kalman gain
x = x + K · Inn Correct the prediction of the state
P = A · P · A’ – K · C · P · A’ + Sw Calculate the covariance of the prediction error

The C matrix is the one that extracts the ouput from the state matrix. In our case, this is (1 0)’ :

alpha = C · x

Sz is the measurement process noise covariance: Sz = E(zk zkT)

In our example, this is how much jitter we expect on our accelerometer’s data.

Sw is the process noise covariance matrix (a 2×2 matrix here): Sw = E(x · xT)
Thus: Sw = E( [alpha bias]’ · [alpha bias] )

Since only the diagonal elements of the Sw matrix are being used, we’ll only need to know E(alpha2) and E(bias2), which is the 2nd moment. To calculate those values, we’ll need to look at our model: The noise in alpha comes from the gyroscope and is multiplied by dt2. Thus: E(alpha2) = E(u2)· dt2.

These factors depend on the sensors you’re using. You’ll need to figure them out by doing some experiments. In the source code of the autopilot/rotomotion kalman filtering, they use the following constants:

E(alpha2) = 0.001
E(bias2) = 0.003
Sz = 0.3 (radians = 17.2 degrees)

Further reading

22 May 2006, 10:49 | Link |

  1. With a gyro we can determine orientation, but it has drift. So we correct it with accelerometers. When a plane is in a turn, the accelerometers won’t measure gravity, they will measure gravity+centripedal forces. How do you correct this?

    Great set of articles!


    — Brian, 27 May 2006, 16:15 | #


  2. Hi Brian,

    The idea is that turns are usually relatively short in time. This means that the output of the gyroscope will be pretty good in this time-interval (and over a short time-span, the gyro should dominate). In case the plane keeps turning, the gyro-output will be close to zero, so the kalman-filter output will remain more or less the same.

    Keep in mind that all this filtering is still an estimate of the real world.


    — Tom, 27 May 2006, 17:48 | #


  3. I had thought the response time of an accelerometer to be about 1 second. I am guessing a typical turn in an RC airplane is probably 1 – 5 seconds. By the end of the turn, won’t the plane think it is nearly level?

    Have you decided on any hardware (e.g. airframe)?


    — Brian, 28 May 2006, 03:41 | #


  4. I made my tests using the Sparkfun Electronics 6DOF module. This module has 11 sensors outputs, each one being update 366 times a second! The accelerometer’s response time isn’t really limited. The gyroscope usually has a limit, like 300 degrees/sec.

    I’d like to use the Multiplex FunJet as airframe, because the paparazzi project is using the MicroJet, and they are pretty happy about it.

    First version will be without IMU, but with thermophiles to stabilize, like the co-pilot.


    — Tom, 28 May 2006, 16:57 | #


  5. Hey how to find the A & B matrices for altimeter,wind speed data? my pressure sensor board gives 16 bit voltage o/p.


    — samu, 1 August 2006, 06:15 | #


  6. Anyone who can explain me Kalman Filters in the context of Object tracking … thank you


    — Ahmad, 23 August 2006, 05:49 | #


  7. this is a good article for person like me who is not good in math. One question: Is applying KF to the IMU main objective is to acheive flight stability? How do we combine GPS data and IMU to achieve the desired navigation. Any link that might help? Thanks


    — Tawfek, 29 August 2006, 07:05 | #


  8. Great article, but i completely miss the size of P. What’s the matrix dimensioin C*P*C’ + Sz? What would be the expansion to non-algebraic expressions?

    Thanks for the great tips


    — ghint, 4 September 2006, 21:18 | #


  9. Anyone know how to add a simple positivity constraint to one of the states? I have a similar Kalman application (unrelated to UAV’s) where A=[(1 dt),(0 1)] where the velocity state x2 will always (and must) be positive or zero. Of course I could clamp it above zero after the update equation, but is there an smarter way to include it in the Kalman equation formulation?


    — MGakaFox, 22 March 2007, 17:22 | #


  10. Hi,
    I have the the same problem. I need to filter noise from accelerometers. I have the following algorithm, but I do not know how to realize it. Maybe some can help, or maybe someone already has Kalman filter for accelerometers?
    Algorithm:
    %The midpoint acceleration error in terms of the unknown accelerometer parameters is given by:
    dbm=bm*psi+ba+ha*bm+(bm^2)*(FI1-FX1)+db;

    %ba – (3×1) vector of unknown accelerometer biases, normalized to the magnitude of gravity

    %ha – (3×3) matrix |S1 d12 d13|
    ha= |0 S2 d23| |0 0 S3 |

    %S1,S2,S3 – unknown accelerometer scale factor errors.

    %d12,d13,d23 – unknown accelerometer axes nonorthogonalities (misalignments)

    &db – represents other error terms, some of which are observable; for reason of practicality they are only compensated with factory calibrated values.

    %FI1 – (3×1) unknown acceleration-squared nonlinearity for acceleration along the accelerometer input axis

    %FX1 – is a (3×1) unknown acceleration-squared nonlinearity for acceleration normal to accelerometer input axis

    %bm – is a three vector (b1, b2, b3)’ of midpoint components of acceleration in platform coordinates |b1^2 0 0 |
    bm^2= |0 b2^2 0 | |0 0 b3^2|

    %The difference equation for the accelerometers is
    Xa(j)=Fa(j,j-1)*Xa(j-1)+Wa(j-1)

    Fa(j,j-1)=I

    %The 12×1 accelerometer error state vector Xa is composed of
    Xa=[ba’, S1, d12, S2, d13, d23, S3, (FX1-FI1)’]’;

    %Wa(j)~ N(0,Q) is white noise which includes accelerometer modelling and truncation errors.

    %The accelerometer observation is
    Za(j)=Ha*Xa(j)+Va(j)

    Ha=[b1, b2, b3, b1^2, b1*b2, b1*b3, b2^2, b2*b3, b3^2, (1-b1^2)*b1, (1-b2^2)*b2, (1-b3^2)*b3]

    %Va(j)~ N(0,R) is white noise which includes sensor errors.

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    EKF applied to obtain accelerometer estimates.
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %State Vector Extrapolation
    Xa(j)=Fa(j,j-1)*X(j-1)

    %State Vector Update:
    Xa(j)=Xa(j)+K(j)*[Z(j)-Ha(j)*X(j)]

    %Error Covariance Extrapolation
    P(j)=Fa(j,j-1)*P(j-1)*F(j,j-1)’+Q(j)

    %Computing the Kalman Gain
    K(j)=P(j)*Ha(j)’*[Ha(j)*P(j)*Ha(j)’+R(j)]^-1

    %Error Covariance Update
    P(j)=[I-K(j)*Ha(j)]*P(j)*[I-K(j)*Ha(j)]’+K(j)R(j)K(j)’


    — wookX, 17 May 2007, 01:51 | #


  11. Hi,
    Anyone who has written a matlab-file for the kalman filtering of IMU data. I would like to use it for offline-optimization of the constants (E(alpha2), E(bias2), Sz) on my real data for the RS232.
    Thanks


    — Christian Schardax, 24 May 2007, 16:17 | #


  12. What is the definition of the function E(zk zkT)?

    What does the function E(.) stand for here?


    — Parth Shah, 9 September 2007, 07:17 | #


  13. E(.) is a statistical function that represents the expected value.


    — Tom, 9 September 2007, 16:41 | #


  14. Hi,
    someone has written a c-file for the kalman filtering for an 3 axis accelerometer or an 3 axis gyro


    — yvan, 6 November 2007, 15:38 | #


  15. Hi guys,
    I measure 3D acceleration of the center of mass of an athlete by means of a triaxial IMU placed on the sacrum during 100m sprint running. The accelerations are expressed with respect to an earth-fixed global reference frame. I have a problem: during the flight phase, because of skin artifact, I have an accelerometer which does not “respect” the gravitational field: the two horizontal accelerations are not zero!!! And because of the inertia of the sismic mass of the accelerometer, the vertical acceleration only oscillates around g but it is not correctly g. Assuming I can identify on the accelerometric signal when the flight phases occur, I would like to run a kalman filter to recursively adjust the accelerations in the way to obtain zero and -g respectively during the fligth phases, but also a good estimation of the acceleration during the stance phases. I use matlab for calculations but I’ve never implemented a kalman filter in my life. Can anyone of you help me in defying the problem and stuff like that?
    I will be very grateful to you for you precious help.
    Cheers,
    Pietro


    — Pietro, 15 March 2008, 17:25 | #


  16. Tom,

    I am currently working on a project which involves controlling 2 servo motors. I have implemented a working kalman filter and wanted to somehow use the roll and pitch angles to control the motors. Do you have any advice on how to use that data to create pulse widths that could control the speed and direction the motors are moving? Thanks.

    —Tim


    — Tim Hoelle, 21 May 2008, 20:38 | #


  17. Hi,Tom, About Sz and Sw,how to test and decide their value?(as you say:These factors depend on the sensors you’re using. You’ll need to figure them out by doing some experiments) another,How to get to know if the kalman filter run well?i have google them,but,i haven’t get result.
    Thanks for your help.


    — kevin, 15 July 2008, 03:47 | #


  18. The accelerometer output has zero-mean white gaussian noise (at least it should have).

    When we calculate angle from the accelerometer we use the asin or atan function which is not linear.

    So our calculated angle no longer has zero-mean white gaussian noise. Substituting it to Kalman Filter breaks basic assumptions of the filter. Am I right?


    — Maciek, 25 July 2008, 19:59 | #


  19. Hey Tom,

    I had a quick question about this application of a Kalman Filter:

    As I understand it, the u vector represents inputs or controls applied to the system. In the Predict phase, the current state is propagated forward based on the last state, and known inputs into the system. Then in the Update phase, a measurement is made and used to correct the predicted state.

    In your implementation, you are using a gyro measurement as your input vector u, and an acceleration measurement as your output vector z. Strictly speaking, shouldn’t there be no u vector (this is solely a sensing application, there are no controls), and the measurement vector z should contain both measurements (accel and gyro). This setup would necessitate the addition of a gyro measurement to the state vector so that it could be propagated forward.

    Now, I am sure that you know better then I if this will work or not, it just seems to be a bit off in terms of the classical Kalman Filter structure. I am still learning the basics of KF, so if I am off here, please let me know.

    Thanks for the clear step-by-step introduction, this kind of explanation is just what I have been looking for!


    — Nick, 20 August 2008, 17:35 | #


  20. @Maciek
    You are correct, but the first order approximation of arctan(x) is x, so in a rough approximation, it is still lineair (esp. around 0)

    @Nick
    Unfortunately I don’t really understand (from the text) the model you want to use in your proposed filter. You can always email me if you need more help :-)


    — Tom, 20 August 2008, 18:48 | #


  21. Hi Tom,

    Thanks a lot for the concrete approach, it made it so much easier to understand. I just wanted to confirm whether the behavior of my implementation is correct: it appears that most of the matrices stabilize / settle to specific values after a few iterations of the filter; is this correct?

    The only things that still change over the duration of the program are the innovation, x, and y parameters, as expected.


    — Jason, 23 August 2008, 07:12 | #


  22. @Jason,

    Yes that’s correct. I initialize those matrices with values closer to the “stable” values instead of the One matrix.


    — Tom, 23 August 2008, 15:29 | #


  23. Hello I am in need of IMU datas to check my program working or not. If anyone have IMU data please share with me.
    Thanks
    sure..


    — Sure, 4 September 2008, 18:21 | #


  24. tom, great tutorial! but am i suppose to get bias so i can normalise the angle? any help?


    — akmar, 26 September 2008, 18:53 | #


  25. Hi,when i’ve looked at the
    equations above,i found that line “K = A · P · C’ · inv(_s_) “
    why there is a “A” ?


    — Tan, 23 October 2008, 03:25 | #


  26. Hey Tom,
    Thanks for all this info. I also have a IMU 6DOF from Sparkfun. I am trying to figure out how to determine the scale values for the gyro outputs. It is “supposed” to be around 3 mV/deg/sec, how did you determine scale parameters for gyros?
    Thanks,
    Pete


    — Pete, 21 January 2009, 15:10 | #


  27. hi,
    thank you very much for shearing you’r knowledge with us.i am new to this field and i am trying to make a self balancing robot. can u please explain me the part that filter adjust the bias of gyro using accelerometer. please response me as soon as u can
    thank you


    — kasun, 24 February 2009, 03:46 | #


  28. Hi,
    I am trying to figure out how to get yaw. I added a gyro with the 5DOF, and now am trying to get pointed in the right direction for the kalman filtering to get pitch, roll, and yaw. Thanks much


    — frank, 8 March 2009, 19:24 | #


  29. Hi all,
    x = A · x + B · u Update the state x of our model, it means that we calculate aplpha and bias.

    how i can get the constant bias?

    thx


    — kader, 11 March 2009, 16:34 | #


  30. How are the gyro. and accel. oriented for this Kalman filter approach to work? It seems like the output of the accel. would always be zero, since it can measure anything on the z axis.


    — Ferrol Blackmon, 19 May 2009, 23:45 | #


  31. Hi all,

    I have a general questions concerning the Kalman Filter.
    I have to find the position of a moving heavy vehicle(the declination of the vehicle to the ground), which is experiencing trebling and shaking caused by external factors. I can use the data from 1 3D Accelerator and 3 Gyros and a GPS receiver.
    My question is: Is it possible to get the (real)declination angle using the Kalman filter? Has someone worked on a similar case?
    Thank you!


    — LV, 28 May 2009, 14:24 | #


  32. Hi Tom,

    I’m quite new in this area (Kalman Filter). I have one 3D Accelerator and 3 Gyros. With the help of Kalman Filter I must find the exact position of a working machine(declanation of the machine). Therefore I need the initial declination angles. I know that I can get them from the 3D Accelerometer measurements, but I don’t know how. I’ve thought that by double integrating I can get them, but it turns out that it doesn’t work. Is there another way to find them – I mean is there some special kind of transformation matrix I should use? After all what I need is just the matrix with the initial declination angles of the machine.
    10x in advance.


    — ME, 17 June 2009, 18:18 | #


  33. According to the above, we are ignoring state of “trust” of accelerometer readings.
    If so, why would Kalman filter be better then just simplier approach:
    We have:
    1. RollGyroAbsolute0 reading at 0;
    2. RollAccelerometer0 reading at 0;
    3. RollGyroAbsoluteN reading at N;
    3. RollGyroAbsoluteN reading at N;
    Then:
    RollGyroDriftPerReading=((RollGyroAbsoluteN-RollGyroAbsolute0)-(RollAccelerometerN-RollAccelerometer0))/N…

    Simple, fast and no advanced math…

    In reality: we can also do above only when we “trus” accelerometer readings: meaning that 0 and N are actualy sequential states when we don’t have any frame acceleration and/or such accelerations are insignificant (simple check for acceleration vector ~= gravity)...


    — igor1960, 2 September 2009, 23:58 | #


  34. Sorry, in above:
    ...
    3. RollGyroAbsoluteN reading at N;
    4. RollAccelerometerN reading at N;
    ...


    — igor1960, 3 September 2009, 00:00 | #


  35. The aim of this article is to develop a GPS/IMU multisensor fusion algorithm, taking context into consideration. Contextual variables are introduced to define fuzzy validity domains of each sensor. The algorithm increases the reliability of the position information. A simulation of this algorithm is then made by fusing GPS and IMU data coming from real tests on a land vehicle. Bad data delivered by GPS sensor are detected and rejected using contextual information thus increasing reliability. Moreover, because of a lack of credibility of GPS signal in some cases and because of the drift of the INS, GPS/INS association is not satisfactory at the moment. In order to avoid this problem, the authors propose to feed the fusion process based on a multisensor Kalman filter directly with the acceleration provided by the IMU. Moreover, the filter developed here gives the possibility to easily add other sensors in order to achieve performances required.


    — mobile phone accessories, 7 January 2010, 14:57 | #


Name
E-Mail
http://
Message
  Textile Help

|