How to get rid of noise and oscillations using Kalman filtering

Question

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.

Answer 1

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.