Kalman filtering tutorial

[dentompie]

I found out a lot of people are looking for a practical tutorial into kalman filtering, so I wrote one:
http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data

When I have more time I'll add some graphs to illustrate the whole thing!
Comments are welcome :)

Now for people with experience in kalman filtering: I'm trying to figure out how to calculate the covariances. The autopilot sourcecode suggests some values (see my tutorial), but mine are pretty much different. How did I do it? Well, I calculated the covariance in Matlab using data of my IMU module in a pretty steady state. So this gives me (i'm using degrees instead of radians):
measurement covariance noise = cov(accelero_data) ~= 137
process covariance noise: (dt = 1/366)
Q_angle = dt^2 * cov(angular_rate) = dt^2 * 6.2 = 0.006
Q_bias (called Q_gyro in autopilot source code) = ? variance of drift or what?

Help is appreciated here!

Tom

[dentompie]

No comments at all?
Nobody who can help me with the covariance?

[Mr.RC-CAM]

I appreciate that you posted your white paper information. Kalman discussions have bounced around the forum before. But, getting someone with substantial experience with the algorithm, to present working (and practical) models, has not been realized yet. Hopefully you succeed and can do that!

[dentompie]

Well, the plan is to write a series of articles, each one going deeper.
I have a working implementation, but I think throwing the code on the net is pretty useless. These planned articles should provide more insight in the code, so people can adapt it and change it to their own needs.
Anyway, what I mean is that some sign of interest would be encouraging :)

[quantumMechanic]

I've read your tutorial,its great. After reading your notes and hitting the net I've got a pile of reading material to get my head around. I'll be following your developments closely.

Keep up the great work.

Cheers,

Alister

[kd7ost]

Anyway, what I mean is that some sign of interest would be encouraging :)

I think you should feel encouraged even without a lot of feedback from different people. I consider myself a good builder, flyer, designer, and systems integrator. But the high level math you engineers work with have me politely sitting in the back of the class keeping quiet. It's greek to me. I think that might describe a fair number of us. Only a handful will understand what you're doing. I would encourage you to proceed but also let you know that lots of us might like the end product, but don't always know how the magic math does it's work. ;) We'll keep talking amongst ourselves about the light weight stuff or how an end product should function but might just goof you up if we try to talk your math.

Dan

[LukeZ]

Tom, I'll just echo what the others said: You have my encouragement! Tutorials on all aspects of UAV development are in serious need, and most especially (in my opinion) on this Kalman stuff. I am trying to keep up with you but I think your approach of starting easy and moving to hard is good. Actually if you feel up to it you could even start easier than you did. ;) :o But really I think what you have is a good start and I can pick it up if I spend a bit of time with it.

However, as for help in your covariance problem, I'm sorry I can't be of any! You might post on the autopilot list-serv. There seems to be smattering of activity there every once in a while, though I haven't visited in some time. But I think you'd have a decent chance of getting an answer there.


Luke

[Nick Reffold]

Hi Tom,

For many people who are interested in the topic, the math is a little daunting and they might prefer something a little less abstract than matrix algebra.

When I was an air navigator, we were taught and practiced 'limited aids' techniques. This was before the days of inertial nav and GPS!

In these exercises, we used several sources of poor quality data to make a best estimate of our position. Primary aids, like radio beacons that gave range and bearing were disallowed. Instead, we would 'fix' our position by taking 3 position lines. One might be from a range beacon that gave us a circular line of distance. Another from a direction only beacon and a third from, say, a star or sun shot with a sextant.

We would then plot these lines and would inevitably have 3 lines to make a triangle that we called a 'cocked hat'. The first one I ever did was about the size of Yorkshire!

What we then did was to use a graphical technique that allowed us to put an error factor to each piece of data to bias the 'fix' toward the better data and away from the poorer data. It was in effect a statistical bias according to our degree of confidence in each data source.

This whole process would take anywhere between 6 to 18 minutes. Modern navigation computers do something very similar but they do it about 50 times per second!

I would not want to spoil your excellent thread by 'muddying the water' - so if you do not think this helpful, I will pull it.

Best regards to all

Nick

[dentompie]

Thanks to all for replying!
Nick, thanks for this interesting anecdote.

I'm trying to explain the Kalman filtering algorithm as simple as possible. Since the Kalman filter uses matrix algebra, I can't write a tutorial (that helps people understand it) without using it. Transforming the matrix calculation steps into normal "c-code" calculation steps (which I intent in a future tutorial) is only usefull for copying-purposes, and makes modifications almost impossible. Also, it will be nearly impossible to understand the algorithm with those lenghty calculation steps that originate in matrix multiplications.
Please correct me if I'm wrong.

So do most people prefer to see a "c-code" variant of the tutorial, or would some basic recapitulations on matrix algebra be more usefull? :-)

Tom

[LukeZ]

So do most people prefer to see a "c-code" variant of the tutorial, or would some basic recapitulations on matrix algebra be more usefull? :-)This is just my personal opinion, but I'd like to see the math explained more than code. Of course if you want to throw some code out there as well that's fine. But as you said, without an understanding of the underlying principles, pure code doesn't do a person too much good.


Luke

[hugo_vincent]

Thanks for the tutorial -- it's a nice introduction to Kalman filtering.

I'd be interested in seeing a tutorial for Kalman filtering using proper INS "mechanization equations" in the process model.

I'm trying to build such a filter at the moment, using Unscented Kalman filtering and the INS equations from this paper:
http://speech.bme.ogi.edu/publications/ps/merwe04a.pdf

So far I have the unscented Kalman filter part working (in C++), but not the INS stuff in the process model. My state vector is 16 elements long: position (3), velocities (3), attitude as a quaternion (length 4), and the gyro and accelerometer biases (3 each).

I think "explaining" the maths properly is a bit of a lost cause, as a rigorous proof of even the plain-old linear Kalman filter would require a grad-school-level maths and statistics background to follow. Maybe you could go into some of the basic properties of and operations on matrices in another tutorial, e.g. multiplication, inversion etc. (The book "Numerical Recipes in C" might be useful as it contains a reasonable Gauss-Jordan elimination algorithm that you can use of matrix inversion, and it is freely readible online).

Hugo

[dentompie]

That's an interesting paper you have there, Hugo.

Indeed, the maths behind it are not trivial and easy understandable. People who can understand the maths and statistics behind it don't really need help in implementing it as well, so i guess my tutorials will be limited to the more practical sides.
Basic matrix math might indeed be a good idea. I think matrix elimination algorithms are only usefull for bigger matrices, so that won't be part of the kalman basics tutorials :)

Tom

[hugo_vincent]

Basic matrix math might indeed be a good idea. I think matrix elimination algorithms are only usefull for bigger matrices, so that won't be part of the kalman basics tutorials :)
Tom

Really? Only 2x2 and 3x3 matrices have convenient formulae for their inverses. For bigger matrices, even 4x4 AFAIK its easier and faster (not to mention more general) to do an Gauss-Jordan elimination or a LR decomposition.

Are you using one of those 2-state kalman filters for each of your x, y, z axis (one for each gyro/accelerometer pair)?

One other thing you could consider doing a tutorial on, is fixed-point (or scaled integer) math, because most small microcontrollers can't do floating point (although many compilers can emulate it in software *really slowly*).

Another thing would be how to deal with angular discontinuities. e.g. if you did a loop-the-loop how would this kalman filter react? The gyro integrates round past 360 degrees, say to 400 degrees, but the accelerometer now reads an angle of 40 degrees again. Does your filter diverge or become unstable? I use a unit quaternion instead of the set of three angles (roll, pitch, yaw) to get around this problem.

[rabhat08]

hi i am currently doing M.Sc. In embedded systems and controls at University Of Leicester, UK. I am currently working on a univ proj to develop an underwater submarine. We have bought the Sparkfun 6DOF v 2 IMU. It should be arriving ay time this week. I am new to INS. I need to know my position using only an IMU(no GPS since it is not allowed in the competition i am doing it for), in a pool whose dimentions I know. I also know my initial position in the pool. Could anybody guide me as to how i should be getting the position from the data got from the Sparkfun IMU. I have very little time and have just 1 week to get it working. I want to use Quaternion approach as i have read it is one of the most reliable. Kindly guide me as to how i should go about it.

Awaiting reply,

Thanks a lot. I felt relieved when i saw this forum.

[Tom Harper]

Tom,

I'd like to see the math. Program code is another issue.

Tom

[Unterhausen]

the paper that Hugo linked to used a square root formulation of the filter. There can be a problem with numerical accuracy, so they propagte square roots of of the covariance matrix.

Another thing that may be necessary with the cheap mems sensor is to estimate the scale error of the sensors. Adding it as a state could be the easiest way.

[hugo_vincent]

Correct. In my non-square-root implementation, I did see the numerical stability problems mentioned (covariance matrix becoming non-positive definite), so I had to recode it using the square root Unscented Kalman Filter algorithm.

Square-root (and other factorized) implementations also exist for plain Kalman filters and extended Kalman filters. E.g. the KFilter project (http://kalman.sourceforge.net/) uses the UDU' factorization of the EKF.

Good luck!

[Mr.RC-CAM]

Kalman tutorial: http://www.embedded.com/showArticle.jhtml?articleID=9900168

BTW, the author (Dan Simon) has just published an article about non-linear Kalman filter estimations. It is in the July 2006 Embedded Systems Design magazine. See page 38. I suspect this will appear on their web site in a couple of weeks.

[deh6]

One other thing you could consider doing a tutorial on, is fixed-point (or scaled integer) math, because most small microcontrollers can't do floating point (although many compilers can emulate it in software *really slowly*).
This caught my eye as a friend (has his MS Computer Sci and 40 yrs experience with embedded systems) and I have had some spirited discussions about floating point. It has some pitfalls that are usually swept-under-the-rug. Near the beginning of the book of "Recipes in C" there is some text that goes over this and then says that for the rest of the book they will do what everybody else does and ignore the problems.

The basic problem is that wherever there is addition or subtraction (signed) a large loss of precision can arise. Another classic problem is attempting to accumulate small numbers into a sum, where the sum becomes large, with the result that there is no further accumulation because when the small number is normalized in preparation to the addition to the large number it becomes zero. As I remember matrix inversion is a tough case for precision, too.

For embedded systems the answer is to use integer math of sufficient precision and scale at appropriate places. Precision loss aside, for small micros w/o hardware floating point, the amount of time to do floating point is too long for control systems that require fast response. For navigation there is plenty of time, but for stabilizing an airframe things have to move along.

The one BIG advantage of floating point is that one does not have to think (much) about where the decimal or binary point goes, especially for desktop machines where precisions of 64 and 128 bit precision is standard. It takes a lot of cranking on a PIC size machine to do floating point of that much precision.

[Unterhausen]

I ran into a big problem doing controller in fixed point math. I used 32 bit integers. It turns out that using the control algorithm I was using, the sum of the coefficients of the filter was zero to the 13th decimal place, which means that I was missing it using my code. Controller worked fine in Matlab, smashed things in real life. The problem is in a digital control, the sum of the coefficients is your gain at zero frequency, so if your controlled system has a free integrator, you are not controlling it at all. Same code worked fine if I picked coefficients myself. Corresponded briefly with someone who actually got his Ph.D. studying these effects on control robustness.

I don't know about doing a KF using fixed point. Could be a big problem. It has been done though.

[deh6]

Regarding Kalman tutorials--

Rummaging through my stack of "stuff" I ran across a folder with some Kalman filtering articles.

This is my favorite. It uses gas mileage data to show the basic concepts of Kalman filtering. He keeps the math relatively simple. The article is old, but some libraries still have Byte Magazine--

Lobdill, J., 1981; Kalman Mileage Predictor-Monitor, Byte, July, pp. 230 - 248

This one is much much heavier on the math, but I think it is worth mentioning since he does such a nice job of starting with digital filters and building to the Kalman filter. He sometimes add pithy comments at the end a chapter. For example, "In fact, the Kalman filter is seen to be a lowpass filter with an adaptive cutoff frequency." For me this was an ahah! In the one dimensional case a Kalman filter is merely the simplest IIR low pass filter with a scheme for changing the coefficient that sets the cut-off frequency. I've found it easy to get lost in the math, especially the matrix math, and lose sight of the overall process.

S.M. Bozic, _Digital and Kalman Filtering: An introduction to discrete-time filtering and optimum linear estimation_, Edward Arnold, London, 1994.

And just for the record, the mother of Kalman filter articles (pretty difficult for mortals to understand)--

R.E. Kalman, "A New Approach to Linear Filtering and Prediction Problems," Trans, ASME Journal of Basic Engineering, 82:35-45, 1960.

[Unterhausen]

I had Maybeck for my Kalman filtering class. He wrote a really nice book about Kalman filtering that was split into 3 books. The first chapter of the book is on the web, it's not trivial since it has probablility distributions in there, but I like the example he gives. Our first take home exam in the class was to design a continuous time kalman filter. It is just an adaptive linear filter, which is why people ignored Kalman filtering until the digital implementations were practical. Implementing it as a digital filter makes all the difference. It's funny that people have started to dismiss the Kalman filter again -- calling something the unscented Kalman filter (implying the existence of stinky Kalman filters) was rather pretentious. But publishing articles about Kalman filters isn't really the easy path to tenure. Fortunately, it's trivial to mistune an extended Kalman filter, so you can always show improved performance.

[deh6]

Unterhausen,

Yes, Maybeck's on-the-web chapter is good, especially his introductory explanation of the statistics.

I was told that at U. of So. Florida (close to us) they use:
A. Gelb, Ed., 2001, _Applied Optimal Estimation_, MIT Press.
I picked up a copy, but the math is really heavy duty, and the last grad EE course I had was in 1961 so I struggle with the newer math and its notations.

I think for hobby uav purposes the need is for some sample code and accompanying explanations that give a little insight as to what is going on, so dentompie and others attempts to this end are welcome. What makes it difficult is the wide range of math backgrounds in a hobby, whereas in school all the students come in with roughly the same level of skills.

[qqqqqqqqqqqqqqqq]

Hello Tom and everyone,

Hope everyone is fine, and wel done Tom, indeed you have opened a very good tutorial site for newbies like myself.

Tom, I need to implement Kalman filter from the perspective of Object tracking. can you please give some tutorials in this regard also, i understand all the matrix stuff, but need help in object tracking regard, Thanks


Regards
Ahmad

[Unterhausen]

for object tracking, you need to augment states of the object model to your kalman filter. For example, for 2d tracking, I use dx/dt = x velocity and dy/dt = y velocity. You can add as high fidelity an object motion model as you want. I stopped when my results were good enough. My big problem has been targets hiding behind other targets, and trying to model people walking. My sensors see a person as two targets which stop and start in strange ways. Haven't solved this one yet.

[qqqqqqqqqqqqqqqq]

Hello Sir Unterhausen,

I'm a newbie in Tracking so can you please give me a more indepth simple explanation. I'm in the first step of the project, an explanation the way Sir TOM has given earlier about Kalman Filter. Thanks


Regard
Ahmad

[qqqqqqqqqqqqqqqq]

Hello everyone,

Why is everyone so silent here in such an important thread about KALMAN. Ok if you don't want to explain to me the kalman in tracking perspective, ok it fine keep on your discussion about the UAV. its productive to me anways..

Regards
Ahmad

[pixeltje]

Hello everyone,
I'm a newbie in UAV and to get acquainted with the controlling aspect of an UAV I did some web mining and came accross the following thesis "Integration of INS and GPS using Kalman Filtering" by VIkas Kumas. This thesis gives a very good explanation about INS and Kalman filtering. For details including all the c source code check out this site:
http://www.casde.iitb.ac.in/publications/pdfdoc-2004/ and then check all files starting with vikas. I tested the c-code and everything worked out OK.

Regards,
Pixeltje

[stevenh]

New to forum - I am strictly interested in UAV application using low-cost toy/model r/c helis.

What's the likelihood of accomplishing this stuff with differential equations only - in otherwords, and the rest of the world should know, that in the United States, it's likely a lot of us are only Algebra II and some trig.
Our "high schools" really just prepare for kids for either going on to college, or getting a trade.
When the Navy was teaching enlisted technicians electronics or nuclear reactor principles, I was told that they were able to teach the physics WITHOUT calc or matrix math.
Just algebra II, and trig.
I think this is why the robotics thing is kind of stalled too.
The next step is a lot of software that relies upon concepts that are mostly taught using calc and matrix math.
Am I alone in this opinion?

[dentompie]

I'm afraid I don't really understand what you're saying.

As for me, I prefer matrix algebra over differential equations (matrix algebra = algebra 1; differential equations = calculus 3 at least here in belgium :-) )
There are some advanced concepts in the filtering (statistics...) but to use it, you just need to know some basic matrix algebra and very basic linear modeling.

Also note that the linear model used is just a simple discrete version of a differential equation :-)

I'm sure a good teacher could teach you the principles without any advanced math, but to transform the problem into a computer algorithm, you'll need matrix algebra...

[Unterhausen]

the nice thing about a matrix equation is that it represents a lot of equations with only one. But in general, the matrix equations for an INU are pretty sparse, so what you end up with is pretty much just a system of nearly decoupled equations.

I'm not sure exactly what you're looking for here either. I have a Ph.D., studied Kalman Filtering for over a year in grad school, and some of the navigation problem is still not so easy that I could explain it to you without studying some more.

[stevenh]

As always, I am trying to say too much in a simple post. My apologies and let me be focused.

I am looking for a application note or white paper that might give a description that, with some study, would take someone who is really interested through the application of the Kalman filter.
Every time I look at something, it always assumes a level of knowledge I don't have.

Think technician level people - your R&D bench tech who wants to following along...

[stevenh]

A question - is PID control a single axis of control versus Kalman control which seems like more than one axis?

And why would I want to use PID over Kalman for a single axis of control?

[skogsvargen]

PID control and the Kalman filter is complementing each other in an AUV.

The PID algorithm helps you to get the plane to the attitude you want it to have by moving the control surfaces. On PID-loop per axis (roll, pitch, Yaw) and also for speed.

The Kalman filter is kombining different sensor values in a smart way to give you the best possible estimate on what attitude (roll, pitch, Yaw) you have right now.

So the output from the Kalman filter (current roll angle) is together with your control signal (fly level) the input to the PID control.

Please correct me if i'm wrong!

/Magnus

/Magnus

[stevenh]

My first purpose is to only hover or "perch" in position and could assume no big changes like from a gust of wind.
The target platform would start level, and could sense "level" with gyros on x, y, and z axis (pitch, yaw and roll).
This is a fixed-pitch main prop and fixed pitch tail-rotor.
The four outputs:
1. Main rotor prop rate
2. Tail rotor prop rate
3. Main rotor (adjust swashplate position) Position in yaw
4. Main rotor Position in pitch

So the tail rotor is really front nose position of the heli
And while swashplate adjustment is to compensate for tilt, I don't care for now if the heli walks as long as it's level.

Can the gyro's be the only input to the servo's?

[deh6]

Check out the following helicopter stabilization project--
http://autopilot.sourceforge.net/index.html
This group appears hobby oriented.

I typed "model helicopters kalman" in to Google search and got a lot of hits. I got the impression that the work is mostly being done in universities, e.g. a couple of Master theses were about autonomous model helicopters. It may be that the development has not reached the point where there is simplification, and maybe even standardization, in how to do it.

[stevenh]

Thanks for the link.
I just received my Parallax Propeller kit. I got a couple accelerometers - and now realize I didn't get Gyros.

For those who know - would it be practical to "can" a specific Kalman filter implementation for use by others who don't have time/interest to get knee deep into theory.

And I am thinking for "hover" the PID is what I focus on - and for nav, that's what the Kalman is for.

[hugo_vincent]

Hi Stevenh,

Yeah, it would certainly be do-able to "can" a Kalman filter implementation. There seem to be a couple of implementations in C that are sort of, almost "canned". I would definately like to see a easy-to-integrate implementation that you can just call every sample period with the latest measurements, and get back the latest estimates. I am working on a Unscented Kalman filter, but at the moment, it's not even close to being of use to anyone else.

What programming language does your Propellor kit use? Can it run compiled C? Does it have floating-point math support?

Regarding your earlier question of PID vs. "Kalman Control" (I am not aware of such a thing existing...): yes, each PID loop will control one axis. For a helicopter at least, there is quite a lot of coupling between axes, so a matrix control algorithm (at the simplest level, state feedback control in state space) can buy you quite a lot of performance. Like Unterhausen said, a matrix equation is a compact way of writing a lot of equations. So, you don't need to use matrix math if you don't want to. Basically a set of PID-type controllers with their outputs added together can do the same thing. If you like, I can explain this better another time :-)

Hugo

[stevenh]

This Propeller chip doesn't seem to have C support but does have floating point support in the language. So I would have to port any C code.
Thanks for the clarification on the Kalman vs PID.