How to get rid of noise and oscillations using Kalman filtering

I have not studied signal processing at all, so please forgive any ignorance in the following discussion.

I have some noisy position measurements that I've been trying to smooth. I've been attempting Kalman filtering but getting either hf noise or lf oscillation depending on the constants I put in. What should I do?

The full story:

I've used a program which is basically like this (I read briefly about how the Kalman filter works and made my own version):

for z_measurement in samples:
expected.r = old_estimate.r + old_estimate.r * delta_t
expected.v = old_estimate.v + old_estimate.a * delta_t
error = z_measurement - predicted.r
new_estimate.r = old_estimate.r + gain.r * error
new_estimate.v = old_estimate.v + gain.v * error

old_estimate = new_estimate

This worked fine for constant acceleration, but when I have a slowly changing (uncontrolled) acceleration (drag which appears slowly decrease) as another state variable, I had a trade off. As I lowered the position gain, I got rid of high frequency noise, but I got even worse low frequency noise which overwhelmed the signal in the acceleration. The position gain seemed to act like shock absorbers, but let through too much high frequency noise.

I've been trying to learn real Kalman filtering to see if that could help. I do not have a control system, so I'm ignoring B and u. It seems to turn into a computer program like this:

for z_measurement in samples:
x_prediction = A_update * x_old_estimate
P_int_covariance = A_update * P_old_covariance * A_update + Q_process_error
K_gain = P_int_covariance * H_measure * (H_measure * P_int_covariance * H_measure + R_measure_error)**-1
x_new_estimate = x_prediction + K_gain * (z_measurement - H_measure * x_prediction)
P_new_covariance = (Identity - K_gain)* P_int_covariance

x_old_estimate = x_new_estimate
P_old_covariance = P_new_covariance

This works fine for calculating averages, but looks non-trivial to define P, Q, and R right for a real system.

From what I now understand of Kalman filtering:

A is the prediction model of the system. It corresponds exactly to my previous program:

expected.r = old_estimate.r + old_estimate.r * delta_t
expected.v = old_estimate.v + old_estimate.a * delta_t

H is a meaningless function used to access the measured parameter.

expected.r

Q and R are error matrices that define the expected noise/error at each stage, and P defines how error of one state variable increases the error of other variables.

However Q and R are constants, and P and K_gain quickly converges to constants when Q exists. In fact K_gain is the only useful matrix (the others are only used to calculate the eventually constant K_gain).

It seems to me that rather than using dark arts to guess Q and R, you can do it directly by playing with K_gain until it works.

Which a few days later leads me back to my original program.

...

Will persisting and correctly defining Q and R, (or K directly) actually give me gain constants that get rid of my noise and oscillations, or would I be better off trying non-linear gain functions. Alternatively should I put up with the high-frequency noise and use a separate low-pass filter afterwards, but then would it be better to do simple low pass filtering and totally forget the Kalman filtering?

Are there any other filters I should try? I'm afraid that linear filtering will loose too much signal. I tried some quadratic filtering and ended up with oscillations with periods related to the length of the filtering window.

First, I think your motion equations are wrong. Assuming a constant acceleration model, the transition matrix, $\mathbf{A}$, should look like that:
$$\mathbf{A} = \left[\begin{array}& 1 & \Delta_t & 0.5\Delta^2_t \\ 0 & 1 & \Delta_t\\ 0 & 0 & 1\end{array}\right]$$

Next, for your question, if the measurement noise (defined by $\mathbf{R}$) and the model noise (defined by $\mathbf{Q}$) are Gaussian, then the Kalman filter is the linear MMSE. That means that it is the optimal linear estimator in the MMSE sense.
Playing directly with the filter's gain yields a simple exponential averaging of the data.

If you have access, I would recommend Tracking and Data Association by Bar-Shalom and Fortmann. This is an excellent book that covers Kalman filtering.

• Thanks for your help. That should be 0.5 delta_t^2 on the top line, and yes, the full version did include that. I know that Kalman will yield the optimal linear MMSE, but there are no guarantees that a non-linear MMSE could be more optimal. In fact, as turbulent drag is non-linear, and therefore unable to be modelled in a basic Kalman model, I do not know whether the second order noise that I dislike is inevitable in a linear solution. Nov 29 '14 at 11:07
• My other query is that as K_gain quickly converges on a constant solution, isn't Kalman simply an exponential averaging of data once the model factored out? I was surprised when I discovered that the actual measurement error did not feed back into the estimates of Q or R. Nov 29 '14 at 11:18
• Try to look for adaptive Kalman filter or manoeuvre detection (and in particular, noise level adjustment if you get a hold of the book I mentioned)
– ThP
Nov 29 '14 at 14:44
• Thanks for your help, sorry I don't have the rep points required to upvoted your answer. Manoeuvre detection is a useful phrase. I implemented the full Kalman model, but couldn't improve on my smoothing further than manually changing the weightings. I t Dec 8 '14 at 8:59
• In the end, I got acceptable results from simply increasing my delta-t to get rid of measurement noise and hide the system noise. Dec 8 '14 at 9:07