Kalman filter for dummies

Per Svensson · Joined: 03 Oct 2004 Posts: 235 Location: Gothenburg, Sweden

Well,..
Like some of you I have for years jealously looked at those rare guys who knows how to dompt these infamous Kalman Filters (KF) used in some
advanced control and measuring equipment for signal estimation and restoration.
Although I have a university M.Sc in my backpack and many years of math experience I have had respect for the theories.

Now I have spent a few weeks trying to understand the basic mechanisms behind the Kalman filter and how it uses real process models
for optimal function. It is all based on statistics and iterative computation.

I have also written a VB6 demo for my own understanding of how to implement a KF in VB or Bascom.
What I would like to offer you here is a little glimpse of what a KF can do for you in your bascom projects and perhaps that we together can
better understand how it works and what it can be used for. The goal is as always to show that we are stronger as a group than as individuals.

A typical use is for estimation (intelligent guess) of a measured parameter with the help of one or more sensor signals. The real beauty is its ability to
merge observations from several sources into one single result. The result will be a mix of the BEST observations in combination.

Start reading the attached documents, and if you find it interesting but hard to grasp, then you are a candidate for this topic. Smile

If you find it easy -then congratulations - we will need you. If you find it repelling - then read it again, and again and again.
In the end of the day you will see the light...

In the mean time I'll compile an installation package so you can try out my trial application. It is a gyro-supported compass application.
You can also Google "Kalman filter for dummies" or Check Wikipedia if you ain't faint hearted.

Comments Invited! (If I get no hands up, I will probably just drop the subject)
/Per

six1 · Joined: 27 Feb 2009 Posts: 553

respect Per! Keep up the good work!

hgrueneis · Joined: 04 Apr 2009 Posts: 902 Location: A-4786 Brunnenthal

Me...not that far advanced but I use a similar aproach with my laser-video-ranger.
In order to increase resolution a pixel position shift in horizontal and vertical direction depending on the values around the max. measured values with the assumtion of gaussean dropoff in laserlight from the center of the beam.
The process takes some calculation but eliminates the need for an extremly high resolution video camera.
Calculation is quite a bit simpler than KF but just as effective.
Just my 2 bits.
Hubert

luizabbadia · Joined: 11 Dec 2007 Posts: 112 Location: rio de janeiro

Thanks.I hope you post the Bascom example code too , for dummies...like me...

Per Svensson · Joined: 03 Oct 2004 Posts: 235 Location: Gothenburg, Sweden

Here is the VB6 installation of my kalman filter example to start with.
It is an XP compilation, byt will work on Win7 as well. But don't allow the installation to overwrite your more recent DLLs in that case.
Observe that it is an INTEGER implemetation, for speed reasons, So do not try to enter FP numbers.
I hav a verson for FP as well. Behaviour is similar but Integer performance is of course superior (speed)

The default KF noise paramters will work, so all you hav to do is push the RUN button for a ramp stimuli simulation.
After this you can se how it works for Sine, Triangle or Square wave for the same param settings.
Once you have done this it is time to alter nose figures to see how it changes Kalman Filter gain and signal tracking.
Read the documents to understand how to change noise figures to optimize filter characteristics.

In the mean time I will port code to Bascom so you can run an example in the bascom IDE simulator.

Tell me if you want the sources and I will show the most up to date one. Either for VB, Bascom or both.

Good luck!
/Per

luizabbadia · Joined: 11 Dec 2007 Posts: 112 Location: rio de janeiro

Yes,please. Very Happy

Per Svensson · Joined: 03 Oct 2004 Posts: 235 Location: Gothenburg, Sweden

Ok, here comes my Kalman Filter source written for VB6.
It is an implementation based on an excellent article written by Kristian Lauszus, TKJ Electronics last august. See zip file.

It is tailored for integer math which will speed up things considerably when we port it to Bascom.
In VB6 it doesn't matter of course, but I used integer math there as well to prove the concept and see if the algorithm was robust enough
to allow truncation of all decimals.
In order to do that without loosing all numeric resolution I have conditioned that the input signals should be -1000 to +1000 or more.
A normal sensor will typically have full-scale swing of 1000 (10bits) or more so this is really not a limitation.

I also tested a floating point implementation and you can see the difference of running the same input sequence side by side for Ramp, Sine, triangle and Square
stimuli (Well - the noise is not the same of course, but all input signals are of equal shape and level)
As can be seen in file "Kalman example plots.pdf" The filter behaves almost the same using integer math as using FP math.
(The Integer runs are those where no decimals will show up in the boxes for State estimate and Error Covariance.

I believe the code to be correct, but if someone find something peculiar in it, please tell!

My next step will be to try this in an Xmega128 with Magnetometer+Gyro.
I would be delighted if more people did the same. We need lots of experience to gain knowledge of how to best trim these filters,
and OF COURSE how to model them for other purposes.

Another field of interest (my private) wold be to estimate a compass course under heavy sea disturbances in a sailboat.
It is very difficult to filter out wind and sea disturbance as these frequencies lies in the same band as the boat movement itself.
So the system must dynamically "learn" the noise pattern all the time depending on current weather and wind.
That is - to design the smartest possible autopilot for small yachts. Anyone interested?

/Per

luizabbadia · Joined: 11 Dec 2007 Posts: 112 Location: rio de janeiro

Well I think that would be done with the help of tri-axial accelerometer to find the final vector (PYR) and apply the correction to the compass , tri-axial either,right?

Per Svensson · Joined: 03 Oct 2004 Posts: 235 Location: Gothenburg, Sweden

It is far more complicated than just correcting the compass for tilt.
If you use a compass that way without trying to separate the course changes from the wave and wind disturbances
you will soon end up with an empty battery and a worn out steering machine.
The servo will simply kill itself in its attempt to compensate for movements (noise) that an real helmsman would simply ignore
as their average is close to zero.
The problem is that all that noise is of all frequencies. Also those within the regulators normal range.
So the challenges are:
1. Find those large noise contributions that are periodic and try to ignore them (Large periodic waves)
2. Find the aperiodic disturbances as soon as possible and try to make your (PID) regulator compensate for these.
3. Keep track of long time deflections and compensate for these- (I-term)
4. Make sure PID factors are trimmed to make the whole system stable also when provoked heavily.

There are more to like varying sail trim and weight loads it, but these were the main problems Smile

/Per

luizabbadia · Joined: 11 Dec 2007 Posts: 112 Location: rio de janeiro

Ok, I see that it´s more complicated.
But the solution would be a very clever device to predict and compensate among another things.
That would be a very challenging project!
I like that!
But I´m affraid the solution is beyond my knowledge.

Per Svensson · Joined: 03 Oct 2004 Posts: 235 Location: Gothenburg, Sweden

Don't underestimate yourself Smile

It is often more a question of persistence than intelligence.
I consider myself as an educable retard...
/P

Meister · Joined: 27 May 2010 Posts: 319

hgrueneis · Joined: 04 Apr 2009 Posts: 902 Location: A-4786 Brunnenthal

In regard to the compensated compass travel of a boat (ship).
It should not be so hard to get data from one max. tilt angle to the next and also the angle in respect to the heading.
The next thing would be to measure enough samples of the tilt corrected compass per given time interval.
Calculate an average heading from it and correct after the next peak. This would be the initial action.
After that a general correction could be applied that would hold the nominal heading steady until it is calculated different again (because of shifting wind,current waves and so on).
Traveling between Kiel and Oslo (3 month after a ferry went down with over a thousand people) I noticed a repetition of about 9 seconds as the waves pounded against the bottom of the ferry ship (it was a stormy night). Most repetitions would probably be within 30 seconds. Where lengthwise tilts would most likely be the slower ones.
It could also be combined with a GPS or without a compass just with a GPS.
If just with tiltreading max. to max. and a GPS, it would be simpler to calculate the steady correction for the given heading. In that case the position can accurately be checked for the optimum course.
I just hope nobody is so dense as to take a boat trip without a mechanical compass as a backup!?

@Per
You do great work, take the entitled cheers, congrats and prais with pride and don't be so humble, you have earned it!
Hubert

Per Svensson · Joined: 03 Oct 2004 Posts: 235 Location: Gothenburg, Sweden

Thanks for those kind words Hubert.
We all need a little cheer sometimes.

When it comes to the autopilot issue, it is not just the stable reading of the heading that is a problem.
As you say, a GPs can be of great help there but it has to do better than (1 fix/s). A GPS + Gyro is excellent
or a compass + gyro too. The real challenge is to find the boats model parameters on the fly.
That is inertial momentum and rudder gain together with momentum from wind pressure and speed
(Unsymmetric hull or rudder can also be static parameters, candidates for cancelling)
pre-measured model params can of course be used to ease things up, but I find it more challenging to design an autopilot
that will run right out of the box on ANY ship size and type. So to boil it down we need the following functions.

1. A good course estimator
2. A robust model parameter estimator for inertia and rudder gain (a first order system model is ok)
3. A regulator that is steady and robust (PID or FUZZY or other mechanism)
4. A smart steering machine that understand to use as little power as possible.

If there is interest for regulation and control in this forum, perhaps we could set up an interest group.
I assume that the Kix people could have quite a lot of knowledge to contribute for ex.

Anyway. What about the main issue. KALMAN FILTERS !
Virtually no response!
Has anyone tried my sample filter? Any comments?
/Per

Per Svensson · Joined: 03 Oct 2004 Posts: 235 Location: Gothenburg, Sweden

I added a box to the Kalman example where one can add an (undesired) offset to the gyro rate stimuli.
(It is in the stimuli section)
Run the simulation and see how elegantly KF finds the offset and cancel it out within a few samples.

Change from linear to triangle or sine waveform and note that there is no delay. Due to the predict-update mechanism in KF the will not be
a pronounced lag in the response as long as the measured signals can be trusted (low noise) When noise is high KF will tend to trust historic states
higher than measured inputs and thus introduce lag.

Change to square stimuli waveform and see how robust KF is. Even when input signals change violently it will have very little ringing, and will also detect
the gyro offset just as well as in the linear case.

Isn't this amazing?! Smile

/Per

(Zip contains just the exe. I assumed that you already installed the runtime from previous posts)