MAV-blog : Kalman filtering of IMU data http://tom.pycke.

be/mav/71/kalman-filtering-of-imu-data

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

Kalman filtering of IMU data » Related articles

Artificial horizon
Introduction First flight with my autopilot
Autopilot module - revision 2
To many of us, kalman filtering is something like the holy grail. Indeed, it
Kalman demo application
miraculously solves some problems which are otherwise hard to get a hold
Testing Kalman filters
on. But beware, kalman filtering is not a silver bullet and won’t solve all of
Sparkfun's 5DOF
your problems! How to desolder a chip
Autopilot eval board
This article will explain how Kalman filtering works. We’ll use a more practical
Gyroscope to roll, pitch and yaw
approach to avoid the boring theory, which is hard to understand anyway.
Accelerometer to pitch and roll
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 » MAV/UAV blog navigation

algebra may also come in handy :-) Frontpage

About
Basic operation Contact
Articles:
Kalman filtering is an iterative filter that requires two things.
Testing Kalman filters
First of all, you will need some kind of input (from one or more sources) that
Micropilot MP2028 Review - Part 1
you can turn into a prediction of the desired output using only linear Gyroscope to roll, pitch and yaw
calculations. In other words, we will need a lineair model of our problem. Accelerometer to pitch and roll
Secondly, you will need another input. This can be the real world value of the Overview: autopilot modules
predicted one, or something that is a good approximation of it. Overview: commercial IMU's

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
» Staying tuned
input.
MAV: XML | Atom
Our simple model All: XML | Atom

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

Ads by Google
Kalman Filter Imu
Accelerometer
Axis Tilt Sensor
Imu

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

1 de 13 08/06/10 15:27
MAV-blog : Kalman filtering of IMU data http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data

the input. In our case this will be the gyroscope’s data. Remember how we
integrate? We just add the NORMALIZED measurements up:
vertical gyroscope
alpha k = alpha k-1 + (_u_ k – bias) IMU, VG, AHRS, MEMS
& FOG Sensors
We need to include the time between two measurements (_dt_) because we Measure Static &
are dealing with the rate (degrees/s): Dynamic Motion
www.memsic.com
alpha k = alpha k-1 + (_u_ k – bias) * dt
IMUs and Inertial
We rewrite it: Sensors
6DOF inertial
alpha k = alpha k-1 – bias * dt + u k * dt measurement units,
inertial sensors &
Which is what we have in our matrix multiplication. Remark that our bias
custom solutions
remains constant! In the tutorial on gyroscopes, we saw that the bias drifts. www.memsense.com
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 IMU Orientation
second input)! Great! Sensor
Miniature inertial
Wrapping it all up measurement unit low
cost, triaxial inertial
Now all we need are the bits and bolts that actually do the magic! These are sensors
www.MotionNode.com
some formulas using matrix algebra and statistics. No need right now to
know the details of it. Here they are:
Integrated Gyro &
Proc.
u = measurement1 Read the value of the last measurement
Angle(+/-1024deg)
x =A·x +B·u Update the state x of our model Rate +/-300 /s MEMS
Read the value of the second measurement/real value. Gyro- FPGA Loop
y = measurement2 Here this will be the angle calculated from our
Controller
www.arrow3i.com
accelerometer.

Calculate the difference between the second value and

Inn = y – C · x the value predicted by our model. This is called the
innovation

s = C · P · C’ + Sz Calculate the covariance

K = A · P · C’ · » Archive
Calculate the kalman gain
inv(_s_)
2009
x = x + K · Inn Correct the prediction of the state November
P = A · P · A’ – K · C · September
Calculate the covariance of the prediction error
P · A’ + Sw June
January
The C matrix is the one that extracts the ouput from the state matrix. In our 2008
case, this is (1 0)’ : December
November
alpha = C · x
October
February
Sz is the measurement process noise covariance: Sz = E(z k z kT)
January

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

data. December
November
Sw is the process noise covariance matrix (a 2×2 matrix here): Sw = E(x · October

2 de 13 08/06/10 15:27
MAV-blog : Kalman filtering of IMU data http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data

August
xT)
July
Thus: Sw = E( [alpha bias]’ · [alpha bias] )
June
Since only the diagonal elements of the Sw matrix are being used, we’ll only April

need to know E(alpha2 ) and E(bias 2 ), which is the 2nd moment. To 2006

calculate those values, we’ll need to look at our model: The noise in alpha November
2 2 October
comes from the gyroscope and is multiplied by dt . Thus: E(alpha ) =
August
E(u 2 )· dt2 .
July

These factors depend on the sensors you’re using. You’ll need to figure June

them out by doing some experiments. In the source code of the May

autopilot/rotomotion kalman filtering, they use the following constants: March

2005
E(alpha2 ) = 0.001 November
E(bias 2 ) = 0.003 May
Sz = 0.3 (radians = 17.2 degrees) February
January
Further reading

Another practical approach

Theoretical introduction
Website on the kalman filter

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 de 13 08/06/10 15:27
MAV-blog : Kalman filtering of IMU data http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data

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?

4 de 13 08/06/10 15:27
MAV-blog : Kalman filtering of IMU data http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data

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, 16: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.

5 de 13 08/06/10 15:27
MAV-blog : Kalman filtering of IMU data http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data

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

6 de 13 08/06/10 15:27
MAV-blog : Kalman filtering of IMU data http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data

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, 14: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, 16: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 | #

7 de 13 08/06/10 15:27
MAV-blog : Kalman filtering of IMU data http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data

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

8 de 13 08/06/10 15:27
MAV-blog : Kalman filtering of IMU data http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data

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

9 de 13 08/06/10 15:27
MAV-blog : Kalman filtering of IMU data http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data

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, 14: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, 02: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, 18: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, 15: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.

10 de 13 08/06/10 15:27
MAV-blog : Kalman filtering of IMU data http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data

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;

11 de 13 08/06/10 15:27
MAV-blog : Kalman filtering of IMU data http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data

...

— 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, 13:57 | #

36. Hello
I’m French …and I apreciate very much your explaination , I have an
interrogation , if S(w) and S(z) are define as constante then a few
iteration ago the Kalman gaib will bee also a Constante , and in this
case where is the interest of this methode ? , certainly I make a
mistake can you give me some explaination thanks very much….

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

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

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

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

12 de 13 08/06/10 15:27
MAV-blog : Kalman filtering of IMU data http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data

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

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

Name
Remember

E-Mail

http://

Message

Textile Help
Preview

© Tom Pycke. Sommige rechten voorbehouden

13 de 13 08/06/10 15:27