kalman_pag

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

» Related articles

Artificial horizon

First flight with my autopilot

Autopilot module - revision 2

Kalman demo application

Testing Kalman filters

Sparkfun's 5DOF

How to desolder a chip

Autopilot eval board

Gyroscope to roll, pitch and yaw

Accelerometer to pitch and roll

» MAV/UAV blog navigation

Frontpage

About

Contact

Articles:

Testing Kalman filters

Micropilot MP2028 Review - Part 1

Gyroscope to roll, pitch and yaw

Accelerometer to pitch and roll

Overview: autopilot modules

Overview: commercial IMU's

» Staying tuned

MAV: XML | Atom

All: XML | Atom

» Sponsoring

Ads by Google

Kalman Filter Imu

Accelerometer

Axis Tilt Sensor

Imu

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

Search

MAV-blog : Kalman filtering of IMU data http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data

1 de 13 08/06/10 15:27

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’ + SwCalculate 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 ·

» Archive

2009

November

September

June

January

2008

December

November

October

February

January

2007

December

November

October

vertical gyroscopeIMU, VG, AHRS, MEMS& FOG SensorsMeasure Static &Dynamic Motionwww.memsic.com

IMUs and InertialSensors6DOF inertialmeasurement units,inertial sensors &custom solutionswww.memsense.com

IMU OrientationSensor

Miniature inertialmeasurement unit lowcost, triaxial inertialsensorswww.MotionNode.com

Integrated Gyro &Proc.Angle(+/-1024deg)Rate +/-300 /s MEMSGyro- FPGA LoopControllerwww.arrow3i.com


2 de 13 08/06/10 15:27

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

Another practical approachTheoretical introductionWebsite on the kalman filter

22 May 2006, 10:49 | Link |

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

Great set of articles!

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

1.

Hi Brian,

The idea is that turns are usually relatively short in time. This meansthat the output of the gyroscope will be pretty good in thistime-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 realworld.

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

2.

August

July

June

April

2006

November

October

August

July

June

May

March

2005

November

May

February

January


3 de 13 08/06/10 15:27

I had thought the response time of an accelerometer to be about 1second. 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 nearlylevel?

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

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

3.

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

I’d like to use the Multiplex FunJet as airframe, because the paparazziproject 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 | #

4.

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

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

5.

Anyone who can explain me Kalman Filters in the context of Objecttracking … thank you

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

6.

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

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

7.

Great article, but i completely miss the size of P. What’s the matrixdimensioin C*P*C’ + Sz? What would be the expansion tonon-algebraic expressions?

8.


4 de 13 08/06/10 15:27

Thanks for the great tips

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

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

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

9.

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

%ba – (3×1) vector of unknown accelerometer biases, normalized tothe 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 factorycalibrated values.

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

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

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

%The difference equation for the accelerometers isXa(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 ofXa=[ba’, S1, d12, S2, d13, d23, S3, (FX1-FI1)’]’;

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

10.


5 de 13 08/06/10 15:27

%The accelerometer observation isZa(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 ExtrapolationXa(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 ExtrapolationP(j)=Fa(j,j-1)*P(j-1)*F(j,j-1)’+Q(j)

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

%Error Covariance UpdateP(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 | #

Hi,Anyone who has written a matlab-file for the kalman filtering of IMUdata. 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 | #

11.

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 | #

12.

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

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

13.


6 de 13 08/06/10 15:27

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

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

14.

Hi guys,I measure 3D acceleration of the center of mass of an athlete bymeans of a triaxial IMU placed on the sacrum during 100m sprintrunning. The accelerations are expressed with respect to anearth-fixed global reference frame. I have a problem: during the flightphase, because of skin artifact, I have an accelerometer which doesnot “respect” the gravitational field: the two horizontal accelerationsare not zero!!! And because of the inertia of the sismic mass of theaccelerometer, the vertical acceleration only oscillates around g but itis not correctly g. Assuming I can identify on the accelerometricsignal when the flight phases occur, I would like to run a kalman filterto recursively adjust the accelerations in the way to obtain zero and-g respectively during the fligth phases, but also a good estimation ofthe acceleration during the stance phases. I use matlab forcalculations but I’ve never implemented a kalman filter in my life. Cananyone 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, 16:25 | #

15.

Tom,

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

—Tim

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

16.

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

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

17.


7 de 13 08/06/10 15:27

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

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

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

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

18.

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 appliedto the system. In the Predict phase, the current state is propagatedforward based on the last state, and known inputs into the system.Then in the Update phase, a measurement is made and used tocorrect the predicted state.

In your implementation, you are using a gyro measurement as yourinput vector u, and an acceleration measurement as your outputvector z. Strictly speaking, shouldn’t there be no u vector (this issolely a sensing application, there are no controls), and themeasurement vector z should contain both measurements (acceland gyro). This setup would necessitate the addition of a gyromeasurement to the state vector so that it could be propagatedforward.

Now, I am sure that you know better then I if this will work or not, itjust seems to be a bit off in terms of the classical Kalman Filterstructure. 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 ofexplanation is just what I have been looking for!

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

19.

@MaciekYou are correct, but the first order approximation of arctan(x) is x, soin a rough approximation, it is still lineair (esp. around 0)

@NickUnfortunately I don’t really understand (from the text) the model youwant to use in your proposed filter. You can always email me if youneed more help :-)

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

20.


8 de 13 08/06/10 15:27

Hi Tom,

Thanks a lot for the concrete approach, it made it so much easier tounderstand. I just wanted to confirm whether the behavior of myimplementation is correct: it appears that most of the matricesstabilize / settle to specific values after a few iterations of the filter; isthis correct?

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

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

21.

@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 | #

22.

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

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

23.

tom, great tutorial! but am i suppose to get bias so i can normalisethe angle? any help?

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

24.

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

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

25.

Hey Tom,Thanks for all this info. I also have a IMU 6DOF from Sparkfun. I am

26.


9 de 13 08/06/10 15:27

trying to figure out how to determine the scale values for the gyrooutputs. It is “supposed” to be around 3 mV/deg/sec, how did youdetermine scale parameters for gyros?Thanks,Pete

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

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

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

27.

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

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

28.

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

how i can get the constant bias?

thx

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

29.

How are the gyro. and accel. oriented for this Kalman filter approachto 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 | #

30.

Hi all,

I have a general questions concerning the Kalman Filter.

31.


10 de 13 08/06/10 15:27

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

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

Hi Tom,

I’m quite new in this area (Kalman Filter). I have one 3D Acceleratorand 3 Gyros. With the help of Kalman Filter I must find the exactposition of a working machine(declanation of the machine). ThereforeI need the initial declination angles. I know that I can get them fromthe 3D Accelerometer measurements, but I don’t know how. I’vethought that by double integrating I can get them, but it turns outthat it doesn’t work. Is there another way to find them – I mean isthere some special kind of transformation matrix I should use? Afterall what I need is just the matrix with the initial declination angles ofthe machine.10x in advance.

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

32.

According to the above, we are ignoring state of “trust” ofaccelerometer 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” accelerometerreadings: meaning that 0 and N are actualy sequential states whenwe don’t have any frame acceleration and/or such accelerations areinsignificant (simple check for acceleration vector ~= gravity)...

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

33.

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

34.


11 de 13 08/06/10 15:27

...

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

The aim of this article is to develop a GPS/IMU multisensor fusionalgorithm, taking context into consideration. Contextual variables areintroduced to define fuzzy validity domains of each sensor. Thealgorithm increases the reliability of the position information. Asimulation of this algorithm is then made by fusing GPS and IMU datacoming from real tests on a land vehicle. Bad data delivered by GPSsensor are detected and rejected using contextual information thusincreasing reliability. Moreover, because of a lack of credibility of GPSsignal in some cases and because of the drift of the INS, GPS/INSassociation is not satisfactory at the moment. In order to avoid thisproblem, the authors propose to feed the fusion process based on amultisensor Kalman filter directly with the acceleration provided bythe IMU. Moreover, the filter developed here gives the possibility toeasily add other sensors in order to achieve performances required.

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

35.

HelloI’m French …and I apreciate very much your explaination , I have aninterrogation , if S(w) and S(z) are define as constante then a fewiteration ago the Kalman gaib will bee also a Constante , and in thiscase where is the interest of this methode ? , certainly I make amistake can you give me some explaination thanks very much….

— NICOLAY, 15 March 2010, 10:01 | #

36.

Hello,I’d like to introduce you a software named ‘Visual Kalman FIlter’. It’svery useful. Would you have a try? www.luckhan.com/kalman.php

— oldjack, 24 March 2010, 10:38 | #

37.

Hi Tom,i tested your code, and i have a lotof of vibration drift (accelerometer).

Did you tested the filter with the motor vibration?

Thx

— Kadero, 3 May 2010, 19:41 | #

38.


12 de 13 08/06/10 15:27

Hi Tom,i am new to avionics,just started 2 weeks ago.i understood kalmanfilter but am still confused how to formulate a complete code out ofit.can u mail me links or other study reference to start with testing..

— Aniket, 5 June 2010, 19:47 | #

39.

Name Remember

E-Mail

http://

Message

Textile Help Preview

|

© Tom Pycke. Sommige rechten voorbehouden


13 de 13 08/06/10 15:27

kalman_pag

Documents