Add your own alternative version
Stats
186.5K views 12K downloads 109 bookmarked
Posted
8 Feb 2012

Comments and Discussions



I have the gps data in mobile format provide by mobile cel, how do for implement the Kalman to make my position more equal? (the data of gps change every time). Thx for help e sorry for english...





I need use this to improve my acuracy.





Phil
I have been reading your work on the Kalman filter. My daughter and I are working
on an exciting project regarding an automated computer trading program and we
would truly like to speak to you.
Donna is a math major (Valedictorian of her class) at Columbia University Master's program in Mathematical Finance (Wall Street quantitative training). I have extensive
experience trading commodities in the floor of the World Trade Center.
Your expertise is exactly what we are in need of. I would be appreciative if you emailed me back or called me so we could briefly chat.
ljones108@aol.com 3362020022
Sincerely, Leo Jones





Hi there,
unfortunatelly the equations and the model are wrong in some significant details, so one is not able to get good results using real sensor data as I did for variometer with BM185 connected to Arduino.
Correct update part is as below (ported to Arduino c++).
If you're interested why try to walk through equations.
I ended up with derivation from classical incremental algorithm, checked with Maxima (nice tool, try it!)
First of all, Z is not a vector but scalar (we're not measuring velocity at all).
Kalman gain is vector of dimension 2 (must be as the state vector is 2dimensional).
Process noise can be nicely modelled as stated below, definitely the mistake was in Reset method, which resetted wrong element (at least process noise matrix should be diagonal)
float Update(float m, float dt)
{
float dt2 = dt*dt;
m_q[0] = (dt2*dt2) / 4;
m_q[1] = (dt2*dt) / 2;
m_q[2] = m_q[1];
m_q[3] = dt2 ;
m_x[0] = m_x[0] + (dt * m_x[1]);
m_p[0] = m_p[0] + dt * (m_p[3] * dt + m_p[2]) + dt * m_p[1] + m_q[0];
m_p[1] = m_p[1] + dt * m_p[3] + m_q[1];
m_p[2] = m_p[2] + dt * m_p[3] + m_q[2];
m_p[3] = m_p[3] + m_q[3];
float y0 = m  m_x[0];
float s = m_p[0] + m_r;
float k0 = m_p[0] / s;
float k1 = m_p[1] / s;
LastGain = k0;
m_x[0] += y0 * k0;
m_x[1] += y0 * k1;
m_p[0] = m_p[0] * (1  k0) ;
m_p[1] = m_p[1] * (1  k0) ;
m_p[2] = m_p[2]  k1 * m_p[0];
m_p[3] = m_p[3]  k1 * m_p[1];
return m_x[0];
}





Hey HoshiKata,
I'm doing the matrix calculations on paper to get a better grasp on what's going on since I'm struggling to implement a Kalman Filter in my Android app for a couple of days now.
I have a few questions concerning the formulas and one concerning the initial state:
1) Am I right that K is actually a 1x2 Vektor, but is [p0/(p0+r0) p2/p0+r0] with p2 = 0, so we can assume it's a single number in the code? That would nullify the whole calculation of question 2 in the update state though, because Xk = Xk + K*Y would essentially mean that x0 = x0 + k0 * y0 and x1 = x1 + 0 *y1, so that would be just x1.
A changing P would of course fix that, but since you only use a single value for k, this kinda goes above my head.
2) I am, as the title says, stuck hat the calculation of Y, which is computed by (Zk  H*Xk). Since H is a 1x2 vector with [1 0], mathematically Y should be Zk  x0, correct? Are you "pretending" we're also measuring velocity by faking a velocity in Zk by saying Zk = [m (mmOld)/dt] so in that case H = [1 0],[0 1], since we're now also "measuring" velocity?
I don't get how you are subtracting m_x1 for y1.
3) As I said, I'm trying to implement this on an Android phone. It's for an App that is supposed to derive movement speed by getting GPS updates, so I would calculate the speed for every GPS update, e.g. every second.
I'm getting an accuracy value for each GPS measurement from the LocationProvider in the phone, so now I'm thinking about the adequate values for the initiation/reset.
qx I would initialize with the accuracy I get from the GPS, and since I'm not measuring velocity directly, can I just use the accuracy value I get from the GPS as an initial value for gv? Is that a sensible value?
The next two values I don't really know how to position. R is the accuracy of measurement, which should also be the accuracy of the GPS, right? In another question thread here, you have initialized r and pd with values that where 0.qx and 0.qv respectively.
Shouldn't pd also be initialized with the value for the accuracy of the first GPS measurement I get from the phone? Since this is the initial variance, shouldn't the "real" variance I get for the first measure be the most accurate variance?
The init value for R I don't really get, since this is the uncertainty of the measurement. I feel like all 4 values could(should?) be, in the case of the android phone, initialized with the same value, the initial accuracy of the first measurement from the GPS, or am I thinking something wrong here?
Thanks a lot in advance,
Sami





I try to implement kalman filter to a problem of mine. Estimating Vehicle arrival time to point B from Point A. Suppose i have the pass data of this route. How can i predict the arrival time using kalman filter?





Hi HoshiKata,
We need to implement a simple form of KF for GPS data gathered by a GPS GSM tracker deployed on wild animals. The implementation should take place by the microcontroller's firmware of the GPS tracker within a short time frame just after the tracker got its first valid 3D GPS fix and before the whole device falls in deep sleep again to save on power consumption from the battery.
Since the movement of the animals is mostly slow compared to driving cars and movement (velocity) can be 0 when the animals rest, I guess the regular linear Kalman filtering used in GPS car trackers can not be used.
I have heard of Extended Kalman Filtering and also fusion with accelerometer data. Can you please tell me what would be possible for our purpose without accelerometer data fusion and with such fusion?
Thanks in advance for your help!
Best wishes,
3dotter,
Netherlands





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





Dear Phil,
Thanks so much for your time and your interest as well. I really appreciate it so much.
I understood that you will accept or reject a position based on the PDOP/HDOP itself. But in the real world , PDOP is not that decent measurment of accuracy. I collect data with GPS with AGPS enabled, thus inside a building , the signal is quite small , the GPS logger tried to get any data based on the AGPS data, thus "proposes" a location which is very far away from my actual location ( may be 500 meter) away. I need only to remove these data .
I have read this article which I think (by checking the graphs ) that have the same issue I have , and I think it is resolved. This article is at :
http://www.ipcsit.com/vol26/7ICTTE2012T012.pdf
I appreciate your advice if I can use your code to implement their approach . I think it will be a good point for your nice article too. .
Thanks, gain for your time and interest.
Waleed.





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





Sorry, just getting around to check the comments.
So the filter is artificially generating a "velocity" based on the position measurements.
A few people have found this confusing, but basically, the idea is your apparent velocity is (x  xold)/dt, which is very different than the instantaneous velocity.
But, it is a decent estimation of velocity.
So, in some cases things are scalar, in others it is a 2x2 or a 2x1 vector.
Phil





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.





I believe your right.
If I have the time I'll double check then make the changes.
If someone more senior here wants to do it that would be cool with me
Thanks for reading, and thanks for helping things improve
phil





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.





Yes, your right,
I've got more things to post but I've found formatting things and getting the right spacing hard.
Had lots of help from Code Project moderators and such.
Super support Without it I couldn't have posted it as well as I did.
But I need to get better to fix things like that
phil





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






Seriously useful introduction. Well done!







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

