Hello,
I am trying to use a Kalman filter to smooth a noisy signal. My signal is actually comprised by two components, i.e., my signal has two dimensions, since it represents the coordinates of a device in movement inside a room. By means of some algorithm, I can estimate the coordinates (x, y) of the device. However, I get some "jitter" that I want to remove in order to get smoother data.
I have employed a simple model for the motion of the device:
r(t2) = r(t1) + u(t1)*(t2-t1),
which in a discrete-time domain is equivalent to:
r[k] = r[k-1] + u[k-1],
where:
"r" is the current position (x, y),
"u" is the instantaneous velocity (ux, uy), measured as: u(k-1) = r(k-1) - r(k-2).
If the time step between consecutive samples is small enough, this model should be rather accurate, even if the motion is not uniform, except for a model noise: "w" with variance "Sw".
Starting from the state and measurement equations:
r[k] = Ar[k-1] + Bu[k-1] + w[k-1] {State equation}
z[k] = Hx[k] + v[k] {Measurements}
where "v" is the measurement noise, with variance "Sv", I have derived the time and measurement update equations, from the theory of Kalman filter.
The results obtained so far are good enough, but I am not sure if I have applied correctly the Kalman filter theory for this particular problem.
My questions are:
- Is this model appropriate?
- Is it correct to say that the velocity "u" is my "driving function" or "model input"?
- Since my measurements are already the current positions except for the noises, i.e., there are no transformations, is it correct to assume matrices A, B and H as identity or unit matrices (A = I, B = I, H = I)?
I am getting better results setting, for example, B = 0.7 * I for some particular "Sw" and "Sv", which means that my model should be actually r[k] = r[k-1] + 0.7*u[k-1].
Please, I am not looking for Kalman filter theory books or papers, since I have plenty of them, I need recommendations for this specific problem.
Thank you very much in advance.
Best regards,
Luis M. Gato
Dear Luis Miguel,
Indeed for the matrix A these eigenvalues must have a real negative part, for the matrix B these elements of the diagonal must be different from zero when to the matrix H is any one.
Also look at links and attached files in topics.
-How a Kalman filter works, in pictures | Bzarg
www.bzarg.com/p/how-a-kalman-filter-works-in-pictures
-Probabilistic Robotics
https://books.google.dz/books?isbn=0262201623
-Aerospace Navigation Systems
https://books.google.dz/books?isbn=1119163072
-Kalman Filtering: Theory and Practice with MATLAB
https://books.google.dz/books?isbn=1118984919
Best regards
Hello Luis,
despite the explanation of your problem I am not 100% I got it right:
You are tracking the position of a target, therefore the state:
r = [x, y]
On the other side, you have sensors which allow you to have measurements on the position and velocity of the target. Here you can say that you are using the velocity as the input for the prediction step, while the position is being used as observation in the correction step:
u = [ux, uy], z = [x, y]
The first thing I noticed is that you skipped the time difference in the prediction model, meaning that B is not identity but B = [DeltaT, 0; 0, DeltaT]. Also notice that as v is your measurement you use it as you get it (not using u[k-1] but u[k] instead): Thus, your motion model would be as follows:
x[k] = x[k-1] + ux[k] DeltaT[k] + wux[k] DeltaT[k]
y[k] = y[k-1] + uy[k] DeltaT[k] + wuy[k] DeltaT[k]
Also, beware that while modelling the noise of your velocity observations, you should define it in terms of "how much the noise grows over time", and therefore the velocity noise w should be given in terms in [m/s2], which is important as in one of the equations that you wrote down, the Jacobian of the velocity noise was forgotten:
r[k] = Ar[k-1] + Bu[k] + Cw[k], A = I2x2, B = DeltaT I2x2, C = DeltaT I2x2
this system is still linear, so perfect! Something that you should bear in mind is that you should NOT actually add that w[k] in the state when you are updating it! Remember that such noise is white and zero mean, so it makes no sense using it in f. Where you actually account for it is while updating the P matrix, making the uncertainty to grow taking into consideration the noise.
P[k] = A P[k-1] A + C Sw
Regarding the measurement update, you described it perfectly. Again, just remember not to actually add that measurement noise to the equation {Measurements}.
All of those were comments related to what you did but... What I would actually do is to track the velocity as also part of the state, as this way you can smooth it way more efficiently. This way, the state would become r = [x,y,ux,uy], the prediction would not have any input (but still you need to make the uncertainty grow within time), while in the correction update I would consider both position and velocity measurements as my inputs, becoming z = [x,y,ux,uy] my measurements, and the H matrix would become H = I4x4. In conclusion, my proposal would look something like this:
r = [x,y,ux,uy];
f--> x[k] = x[k-1]+ux[k-1] DeltaT; y[k] = y[k-1]+uy[k-1] DeltaT; ux[k]=ux[k-1]; uy[k]=uy[k-1] ---> x[k] = Ax[k-1] + Cw[k-1]
A = [1, 0, Delta T, 0; 0, 1, 0, Delta T; 0, 0, 1, 0; 0, 0, 0, 1]. C = DeltaT I4x4, Sw => diagonal matrix with the process noise of position and velocity (4x4 matrix, whose units are m/s and m/s2 for position and velocity respectively).
With regards with your correction step:
z[k] = Hx[k] + v[k]; z = [x,y,ux,uy]; H = I4x4
As well, I would suggest the most basic but effective Kalman filter paper, as the explained example is really similar to yours ;)
I hope I could help you, Luis Miguel :)
Best regards,
Daniel
Hello, Daniel.
Thank you very much for your time and your answer. I won't be able to dowload that pdf at the moment since my internet connection is really poor today.
I must say your explanation was very complete. As I suspected, my model wasn't really well posed. Now I will try your recommendations.
However, the simple model I presented before was giving me somehow nice results. See the following figure, for example. It corresponds to a circular trayectory. The blue line is the actual path, the red line is the measured data, and the black line is the filtered response applying the model I presented above. Seems to be quite well.
Now I will try your proposal and see what I get.
Thanks again.
Best regards,
Luis
Hola Luis:
No soy experto en Filtros Kalman y al parecer te han dado buenas respuestas. Solo quería decirte que leí ejemplos muy parecidos al problema que quieres resolver en este libro: "Kalman and Bayesian Filters in Python" de Roger R Labbe Jr. Puedes agregarlo a tu colección si no lo tienes. Saludos.
- Badges
- Science topic
- Similar topics
- Engineering
- Control Theory