
Comments and Discussions



Hi,
thank you very much for this article and the demo. It is very instructive and the code is very clearly written.
I am not very much into Kalman filters. I am reading a lot about it but still don't get clearly the whole picture.
I have been playing with the code and have a couple of doubts and wondered if you would be so kind as to guide me a bit.
I am trying to apply the Kfilter to a velocity vector of a vehicle. The way I understand it is that as there is not correlation between speed and direction, the Kalman2D is of no use here and I should run two independent filters (kalman1D), one for speed and another for direction. Am I right?
The speed and direction are being obtained through a sensor. I have followed your code line by line and you generate the data and then add Gaussian noise to it. I am assuming that my real data has already noise incorporated.
When you run update the filter in the form1 methods, you set DT to 1. How is this parameter set in real world data? is the difference between current data sample and last data sample time? what magnitude are we talking here... milliseconds, seconds,...?
Finally, it seems to me your article is about standard Kalman filter but you mention several times in answers to this message post 2nd and even 3rd order Extended Kalman filters. Please, correct me if I am wrong...?
All the best,
Adan






i need to find a users position, i have pseudoranges from 12 satellites at different times, i also have gps receivers clock error
and users actual position. I have already made a program to compute users position using least squares approach in matlab.
Now i want to know how do i implement kalman filter equations on pseudoranges and time information provided to compute a
users position. Thanks for any suggestions





This is a complex subject.
There are ways to combine things, but many of these things are independent. So it's kinda up to you.
This is how I'd do it:
Create a filter for each satellite: pseudorange and doppler.
Create another filter for receiver clock correction.
Use the nonlinear least squares to take an estimate of position (last known position, etc). Then use that to compute the error and feed it back through. This part is complex but there are a few online articles on how to do it. Then you iterate this until the solution starts to settle.
Because the data is imperfect and your using a linear estimator in the least squares it won't settle.
Use these things to feedback your pseudorange and receiver errors.
Then use a seperate 2nd order or 3rd order Kalman on your actual posiution, feeding doppler to get the velocities.
There is far less error on doppler than position so if your stationary or moving with little acceleration you get a very
precise position.
What I've seen work wonders is acculumated carrier mixed to pseudorange. Carrier has different atmospheric effects. Combined with single or double differencing you get subcentimeter accuracy.
With pseudorange and doppler you can get sub meter. With only pseudorange your in the meters of accuracy area.
phil





So here my 2 quesitons:
1 if i gave him this code, he can implement it with little modification or he needs to have much knowledge
to use it?
2 if he cant use it, would you develop filter code for me and I pay you, if I have the resources for it?
Thanks a lot!
Cheers from Argentina
modified 3Jan14 21:35pm.





Hello Arturo,
These are small but big questions as there are many options.
1. In my opinion, a decent programer should be able to use the code as is to get started / inspired, if not I haven't done a good enough job writing the article, and I'll help as best I can to provide advice or improve the article.
The code is C#, but I write C# in ways I can port to C++ (in general) easily. So if you code is C++ or C# under 2008, or the free express tools it should require work but low risk / low difficulty.
2. Yes. Advice is free, giving back to the community that has taught me soo much is a good thin.
If it came to significant hours, you could hire me.
There is a huge difference between:
a) getting this code integrated (easy),
b) adjusting to real data (kinda easy, need to understand the theory and play with the code),
c) making it work optimally (hard).
Doing solid Kalman filter design is often a specialty requiring years of practice. You can get "good enough" in a few months to solve basic problems. I'm not an expert, but I've used them successfully a lot.
You might be able to find a graduate student in applied mathematics local to you to help if you need to seriously tune to hardware behavior if you need someone to sit in the lab and help you adapt things.
phil





Phil,
Thanks so much for your prompt answer.
Everything about hiring a programmer is hard for someone that does not program. Is kind of shooting in the dark thing >).
The good thing is I have a gps guidance system that I use to benchmark, so my goal is that with the same gnss reciever data, my software behave same than the one I am using.
I will meet with my guy next week and show this. From there I will see if we can make this work with few questions or I will contact you to see how much and if i can afford it.
Please send me your contact info at agentili@plantium.com.br
Thanks again!
Cheers!





Thank you, sir, for the informative post.
I am following in your footsteps as it were.
I recently put a PID algorithm into practice to regular an ondevice thermostat in a chemistrytype application. The PID itself wasn't that fancy, but what I did do was methodinject the variability ("k"onstants and such) via C++ Boost signals/slots. Which yielded very maintainable code with the concerns nicely separated and close to the problem area where they are actually visible and applicable.
Now we are looking at a firstorder filter such as Kalman for another part of the system, probably with similar extensibility in mind.
Again, thank you, sir.





Glad you liked it.
I'm just learning Boost signals now. I never needed to do much with Boost till recently. Much nicer than Qt in my opinion.
I haven't seen a good Boost signals article.
phil





Yessir, Boost.Signals (boost::signals2) are pretty good IMO, especially once you get past the function<> and bind() syntax.
Personally, I usually like to define a "handler" function typedefinition with a "C# .NET EventArgsesque" parameter, usually const event_args& but sometimes event_args& depending on what I want to accomplish (i.e. with PID, where I pass along results and/or signal for parameters, measurements, etc), it's a great way to handle methodinjection to decouple concerns. It's all pretty much the same, the closest analog I know is the C# .NET event model with +=, = add/remove handlers. I think there was even a boostextension at some point but which is a bit obsolete which modeled the same style eventinterface as a C# .NET app; last glance it was broken with recent Boost versions.
But, technically, if you like living more loosely you can define a function header that receives/returns anything. Another application is a messageunwrapperstatemachine that returns true/false whether it has unwrapped a message. Most of the statehandlers return false with the terminus returning true when it has a message; but all of which receive the next byte.
Thanks.





Dear Sir ,
Although I am not specialist , it looks very intersting. so thanks so much for sharing this great work.
I have one question. I am recording movement by a GPS logger, that stores lat,long,HDOP,PDOP,Speed . it works excellent. but the problem occurs when the logger is inside a closed location , the gps logger get data with very bad accuracy, ie, data jumps in a mad way. I want to filter these data to extract the correct points and remove noisy data. Again , All inputs are taken from the gps logger itself, with no accelerometer. How to do it (in simple way). You can just draw me the outlines , and I will try to understand.
If you want these gps data or the photo for the track , I can send you them .my mail is waleed.makarem@gmail.com
Thanks again for you nice work





Typically, you "coast" the filter.
I think of it this way: a filter expects a certain kind of input. If the input no longer matches it, then it's the wrong filter OR the data is garbage (it happens) in which case you ignore the new data and stay in position, or switch to a different filter.
Wonderful thing of GPS is you have HDOP/PDOP = your dilution of precision or in other words a decent measure of noise.
When HDOP or PDOP exceeds some threshold, don't update the filter, set a flag.
When the filter starts up again and the flag is set, if the new position is far from where you were, use that as the new position and reinitialize the filter.
That would handle coming out the same building door (no reset) or a different door of the same building (need reset).
That would be in my opinion easiest.
It's also the text book thing to do.
In the general sense unless you have accelerometers or you understand why the noise is present and have a strategy (atmospheric / rain / folliage) I think that is more or less the best you could do.
Minor improvements would be CFAR (constant false alarm) where you count the consecutive times the threshold is exceeded (or number of times within an interval of measurments or time) before coasting.
I've been racking my brain, can't think of anything else. Hope it works for you
phil






I think I have found some inconsistency in the equations you provided. I mean this part:
Y = M – H*X
y0 = m  m_x[0];
y1 = ((oldXm)/dt) m_x[1];
S = H*P*H^T + R
Because H = [1, 0] this is easy, and s is a single value not a matrix to invert.
s = m_p[0] + m_r;
K = P * H^T *S^1
K = m_p[0] / s;
X = X + K*Y
m_x[0] += y0 * k;
m_x[1] += y1 * k;
P = (I – K * H) * P
Because H is [1, 0], and K is s scalar in this case.
if H = [1, 0] (i.e. a matrix with one row and two columns) then
K = P * H^T *S^1 is not m_p[0] / s but
[m_p[0] / s ; m_p[2] / s] (a two element vector)
also if K was a scalar, you couldn't substract K*H from 2x2 identity matrix. K must be a 2x1 matrix if H is a 1x2 matrix to perform this substraction.
Also why is Y a twoelement vector? If we measure only position thus M is a scalar, thus M  H*X is a scalar too...
Can you please explain that?
Thank you,
Kamil





Hi,
Would this be any good to detect trees and bushes blowing in the wind? The context i am looking at is motion detection.
thanks for your time.





No idea
I guess it would depend on the sensor.
My guesses are:
accelerometer  periodic motion, probably.
video camera  sure, first identify the feature, then track with kalman.
What kind a sensor?
phil





Hi,
thanks for the reply. The sensor I am using is an IP camera...





Ok,
I'm working on some image tracking code now.
I use OpenCV, and do it with fourier transforms not the integral image methods that are faster.
I do the tracking with Kalman, works great.
I've tested it with liquid level sense in a glass, person moving in a room, etc.
phil





Hi Phil,
I also use OpenCV and Aforge and EMGU. I work with the motion using Aforge s 2Frame differencing and GridMotionAreaProcessing. I get the 'blocks/pixels' that have changed. I look for a certain size. Those that do not fit i ignore. Those that do fit i continue to work with. I am trying to eliminate things like trees blowing in the background but still pick up things that are far away like a car or person etc.
I have isolated the motion changed and view it as a separate image. I am trying to determine whether it is a piece of 'foliage'. i have used template matching, an array of different filters (Aforge) and histograms. Whilst i have some mediocre success it still only 50% at best.
Desperately trying to think of a different approach so now i am looking across the boards for ideas. Do you have any?





Thamks for the great article.
How would you go about calling the code in order to "smooth" gps input ?
I have an application that collects gps data but these are too noisy to use (off a phone) .
Could you please give a small hint towards this ?
Thanks





Your not the only one who's asked that.
When you smooth GPS input, you can either "filter" the raw data (distances and velocities) then calculate your own position, OR you can smooth the resulting position.
On a phone, I'm thinking your smoothing your actual position.
Simplest:
Three separate filters, each 2nd order (position, velocity) one for X, one for Y, one for Z (or lat, long, alt).
You could record your data, pull it into the provided app, calculate decent gains and be done.
Kalman will general improve worst case a good bit and cut the error 1/2 to 1/4 depending on luck, tuning, GPS.
More complex:
Fold in phone's accelerometer data. Gang the filters: Accelerometers + GPS velocity as 2nd order (velocity  accel) filters.
Then take the output of the accelerometer/velocity as filtered velocity. That data will be generally very smooth / accurate.
Take the velocity output and put it into the filter described in the simplest case.
With this and a decent accelerometer and GPS you could get stable submeter results.
Very complex:
Use accumulated carrier and fractional carrier to solve for a stable carrier phase measurement to each satellite by using pseudo range (2nd order).
Kalman filter each satellite's range using carrier, carrier rate, range, doppler (2nd order dynamics but 4x4 matrix).
Use nonlinear least squares to calculate position.
Using methods like that (which are text book documented but hard to find) I got 2.2 mm stable accuracy
Took about a year of work.
Someday I'll post part 2 of this article, codes done, just no time to write the article. Working on it.
phil





Phil thanks for the quick reply. I would probably opt for the option (1) as very high accuracy is not really needed  just a ballpark correction.
I have data streaming in via the GPS and for the time are entered in an array of points :
route[].long
route[].lat
route[].alt
route[].speed
Could you (if possible) post a snippet that would tie this to the code you posted?
Sorry to be a pain but i am not really sure i "grasp" the kalman filter yet !!!
thanks
bob





I just submitted my second article, that shows merging accelerometers w/ GPS.
This is what I'd do with either code:
Kalman1D [] kf = new Kalman1D[4];
for (int i = 0; i < 4; i ++)
{
kf[i] = new Kalman1D();
kf[i].Reset(1,1,0.1,0.1); // That would be ok for meters for GPS,
// not sure what your units are for
// lat, long, alt.
}
// Update as a new route point was added.
// Return the best estimate smoothed route
Route Update(Route route, double dt)
{
Route r = new Route();
r.long = kf[0].Update(route.long, dt);
r.lat = kf[1].Update(route.lat, dt);
r.alt = kf[2].Update(route.alt, dt);
r.speed = kf[3].Update(route.speed, dt);
return r;
}
That would be the simplest. The gains 1,1,0.1,0.1 you'd need to set based on the uncertainty of the GPS.
Example, if your altitude had an uncertainty of say 2 meters STDEV, then the altitude kalman would be roughly 2, 2, 0.2, 0.2
for a first pass guess at a reasonable value.
If your longtitude was in degrees, your uncertainty of a single measurement might be 0.001, so: 0.001, 0.001, 0.0001, 0.0001.
Then you could graph each independently and tune the values for each independently.
I used a loop to "reset" them just because I don't know what units you have.
Hope this helps.
phil





Hi Phil,
Great work thanks,
Quick questions: (mostly yes/no answers so I am not to much of a bore!!)
All my measurements are doubles, the structure of the route[] array is all doubles. (long,lat etc)
1. kf[i].Reset(1,1,0.1,0.1); // That would be ok for meters for GPS,
Is there a param missing here, the last param should be " ,0 " ? I assume so.
2. From what i understand you make a "new Kalman1D()" for each of the readings (lon,lat etc) is what i understand correct ?
3. You call route update on each position received and in the function you call kf.update for each of the individual kf (lon,lat,etc) . You then combine all these three to make a new position (NEW lon,lat etc) . The resulting new position should then be entered in the "big" array of positions ? Again is this correct understanding ?
IF I have understood point (3) correctly does the update() function do an evaluation and update the matrices ?? Also in the parameter "dt" which is as per the code a double should i enter the seconds elapsed from the past reading ?
best regards and THANK you for all yr kind help.
Bob.





1. Yes I think
2. Yes. If your not sure how they are interrelated (merging in velocity) it would be a pain to do otherwise.
3. Maybe.
If route[] is the filtered values then yes.
If route[] is a record of raw input then no.
The update function does the evaluate and updates the matrices.
Yes, seconds from last reading.
You'll need to do a few practical things like reset the filter if you loose incoming data for a long time, etc.
phil





Phil ,
As an example of the data:
latitude="40.7654872465043" longitude="73.9737871031494"
latitude="40.76600730189" longitude="73.9735725264282"





Ok,
Reset(0.001, 0.0001, 0.0001, 0.00001, 0);
Would be my starting guess at parameters
phil





Phil Hi,
I have the following but the correction is minimal.
the array data[] is made of long,lat positions already read by the gps and trying to smooth them via the loop.
Also I notice the any value going in to Dt does not change the outcome.
I am sure i am missing something here ? Any ideas ?
Thanks
Bob
kf[0] = new Kalman1D(); kf[1] = new Kalman1D();
for (i = 0; i < data.Count; i++)
{
if (i == 0)
{
kf[0].Reset(0.001, 0.0001, 0.0001, 0.00001,data[0].Longitude);
kf[1].Reset(0.001, 0.0001, 0.0001, 0.00001,data[0].Latitude);
}
double kalmanLong = 0; double kalmanLat = 0; double dt = 1;
if (i < data.Count  1)
{
dt = data[i + 1].Time.Subtract(data[i].Time).TotalSeconds;
}
kalmanLong = kf[0].Update(data[i].Longitude, dt);
kalmanLat = kf[1].Update(data[i].Latitude, dt);
data[i].Longitude = kalmanLong;
data[i].Latitude = kalmanLat;
}





I want to implement same filter in matlab as i am not familiar with the language used. Kindly tell some theoratical references and equations used to implement it.





<>
Predict:
X = F*X + H*U Rolls state (X) forward to new time.
/>
You use H for your control input matrix in the above equation. However, you also use H as your observation matrix in the update part. With due respect I believe this is not correct. Granted your simulations work because U = 0, thereby cancelling the control input matrix. I would recommend using B for the control input matrix in the predict portion.
X = F*X + B*U
And continue to use H for the observation matrix for the update equations.
Go to Wikipedia and refer to “Kalman Filter”.
modified 6Sep12 1:48am.






Perfect article
very helpfully if you can add example with gyro (3x) and accelerometer (3x).
Not only remove noise and also incerease accuracy with additional data.
Also interesting to check calculation position if gps data lost for short period.
Best regards





Thanks! This is an important contribution to Code Project and the DotNet world.





This is a blast from the past for me. I wrote a Common Kalman Filter simulation back in the 80's for the Air Force in FORTRAN. Sadly, I still don't understand the math...





Hi,
Thank you very much for this very good introduction on KF. I was just wondering whether KF/EKF could be used to solve path following problem. I have generated an optimal trajectory using the threedegreeoffreedom equations of motion. If i want to predict the covariance (i would assume this would grow as a function of time) over a period of time, how should I make a start? Develop the navigation model first?
I am not very familar with kalman filter but I am very keen on learning. Please let me know your suggestions. Thank you.
Crystal





Hi Crystal,
Another reader asked the same question. Once I get a little time I'll post a simulation of using GPS to track a slow moving vehicle.
Kalman is often used for path following problems. It has been used in missile guidance, radar tracking, satellite docking, etc.
I'd start by looking at relative errors in measurement and try to identify the noise related to the physics and the noise of measurement.
Example of what I'm thinking:
Watching a rocket with 2 cameras to recreate trajectory, the pixel dithering noise is measurement noise, the wind buffeting the rocket slightly is the physics or "process" noise. Then I'd set up a pair (X,Y) of Kalman filters each 2nd or 3rd order (3rd would give arching, 2nd would lag slightly). Then use those for each camera to get a trajectory as seen by that camera and then combine that data to get the rocket trajectory. Alternatively (the hard ever so slighly more accurate after months of work and lots of cursing), take all four measurements into the same 3rd order filter (X,Y camera 1, X,Y of camera 2) and use the relationship of how the accuracy seen by one camera effects the estimate of the accuracy as seen by camera 2.
Also instead of solving for what is seen, you could solve for the static parabolic path parameters w/ drag automatically taking time and velocity into account.
Another example: GPS is 5 meter accurate, GPS doppler (velocity) is 0.5 meter accurate. Both are RMS figures so occasionally you do see 1520 meter position errors. By Kalman filtering with velocity smoothing in simulation (2nd order on velocity, 2nd order on position) I got RMS errors of about 0.75 meters (worst case was down to 23 meters). Then adding an accelerometer (0.001 m/s^2) in simulation I was getting 0.200.30 meter errors.
I got 25x the GPS performance with the demo Kalman code, guessing at a couple coefficients over 23 days of effort.
In my prior GPS work I had raw carrier phase measurements (2.2 mm RMS) and used them to lock down the integer ambiguity with a Kalman filter, then when that collapsed to a solution the total GPS position error (real life measurement, confirmed at Dept. Transportation GPS simulator in Cambridge, Mass.) was, no joke 2.2 mm RMS. It took 8 months for me to do, several sets of filters, but it was all possible due to Mr. Kalman.
My suggestion is play with the demo code but pull in your data, try something simple see it's effect, move from there.
The demo code is kinda useful, not industrial strength production ready code, but good enough to learn with.
For a larger Kalman filter you'd need pseudo inverse matrix operations, etc.
Hope this helps.
phil





Below is a copy of the Update equations. May I recommend that for S, the Residual covariance you post it this way:
S = H*P*H^T + R, S = Residual covariance transformed by H+R.
vs.
S = H*P*H^T + RS= Residual covariance transformed by H+R.
Notice how I am recommending separating R & S at the end of the equation so as to avoid confusion. The same holds for the other equations and their respective matrices.
By the way this is a very good article and thanks for posting it. 5 stars.
Update:
Y = M – H*XCalled the innovation, = measurement – state.
S = H*P*H^T + RS= Residual covariance transformed by H+R
K = P * H^T *S^1K = Kalman gain = variance / residual covariance.
X = X + K*YUpdate with gain the new measurement
P = (I – K * H) * PUpdate covariance to this time.






Dear HoshiKata,
Do you think the demo code would work fine to filter a digital accelerometer output? Do you have the same demo code in C ANSI, by the way?
Kind regards,
Marco





I have used Kalman filters for accelerometers. The code in the article could be adapted pretty easily. I have a few questions:
1) Are you attempting to get position or just filter accelerometer?
2) Are you trying to get the gravity vector or just normal motion stuff?
3) Is it gyro stuff (rotation) or acceleration?
If you have extra apriori info like you only care if you are experiancing gravity but need the gravity vector (down) then you can use the fact that gravity is more or less constant: the constant magnitude along with looking for a stable result means you can tune the filter to be very stable and then only use the results when the covariance matrix suggests your very stable. I've done that for magnetic compasses.
If your going to mix with GPS, that works very very well, but the filter design can be hard. Accelerometers tend to have a gausmarkov process, ie. guassian distribution, and the average noise is one of the ratings on that part so you kinda already know some of the information. What you don't know is how movement induced accelerometer errors are alike or dislike GPS movement induced errors so those coefficients are difficult.
If you are just say trying to filter a single accelerometer value and get a smoothed accelerometer value that never seems to lag very much. Then the Kalman 1D code I provided should be more or less perfect. I've used it for video automatic gain control, accelerometers, target tracking etc. Record some sample data where you are steady, then move and then steady, then play that through the Kalman 1D demo, and then tune the gains and your more or less ready to try it. About half the time I've found after simulating there was something I didn't realize about how what I'm doing is actually generating the noise, and later by seeing how the Kalman didn't solve the problem I could find the real source, then apply the correct filter (usually Kalman) to truely solve the problem. So if your doing this as a hobby you might need to iterate once or twice.
I don't have any ANSI 'C' code for this I could share, though the algorithm I used is text book, and the Kalman1D and Kalman 2D code were written with making porting easy in mind.
In any case, I hope this helps.
I've been able to do a lot of things with Kalman filters because system engineers were generous with their time with me, and I'm hoping I can pass some of that info along.
phil





Message Automatically Removed





Seriously useful introduction. Well done!





Kalman filters have always interested me due to my line of work. What a lot of articles fail to point out is how you choose the covariance matrix and choose the initial condition. If you have any insight on how to figure that out it would be an excellent addition to the article. I would also like to see a little discussion on how to choose the Kalman gain matrix.





Both are good points.
There are classic signal processing techniques to measure variance and covariance and isolate the values either recording real data or matching an accurate tuned simulation to real recorded data.
The problem with that, based on my experience (yeah I've tuned a few, but there are people far more knowledgeable) is that's just a starting point.
My personal opinion is the initial covariance should be "big enough" to start, so I typically do 210x the expected noise across the diagonal. That usually for smaller filters makes sense. How ever, if you do make them smaller valued and you take the first value as being exceptionally good (which may be the case for a particular problem) then smaller is better.
The gain matrix .... that too depends, personally I like my output of a Kalman to feel modal, I'm tracking and aggressively filtering, loose track then reestablish. Set things up so I'm tracking w/o noise most of the time, and a certain percentage I'm tracking badly but I flag that. That technique would work great for say a surveying GPS or a mapping GPS. I've done that with great success getting real world 2.2 mm accuracy (single differencing w/ fractional carrier phase at 10 Hz). The technique is wrong if it was for say a tracking algorithm of something moving quickly in flight where you remove a little noise without the possibility of ever loosing track. In those scenarios mild smoothing is in order.
I typically tune dominant things first, and the diagonal of the matrix which corresponds to how a measurement of position can be trusted based on past measurements of position and then move out to cross terms like how my velocity variance tells me how much I can trust my position data.
So, to be honest. I'm not exactly an expert, but having worked with a few I wanted to capture some practical knowledge that I just don't see in the literature, the stuff people were kind enough to take me aside and explain 'cause I was confused.
When I figure out how to update things and get more comfortable I'll try to add something about those two things to the article.





i found this to be to my liking, except for the explanation of the formulas. Where do these come from, and what are their elements ??





There are a ton of ways to write the equations, because you roll forward to predict then update, people often write + or  super scripts, some use different letters, etc.
I used the equations on Wikipedia because I figured anyone else would likely turn there for more information first. It's where I went to remind me how it worked. Other good reference is Gilbert Strang's book on applied mathematics, big fan .
It's a big topic so I figured I'd make it easier for me and others to use simple Kalman filters which lets you do a heck of a lot more than other filter techniques.
phil





Very interesting read, thank you for sharing.





Well nice work there you got my attention and my 5 !






Hopefully you got the email from my earlier post. It got removed when the article was moved.
Either way, I am curious whether or not you will continue elaborating on this topic.
modified 8Feb12 13:36pm.







General News Suggestion Question Bug Answer Joke Rant Admin Use Ctrl+Left/Right to switch messages, Ctrl+Up/Down to switch threads, Ctrl+Shift+Left/Right to switch pages.

Type  Article 
Licence  CPOL 
First Posted  8 Feb 2012 
Views  83,002 
Downloads  6,282 
Bookmarked  88 times 

