Question

I am using the following smoothing function to smooth speed readings from GPS.

void smoothing_init()
{
    k = 0;
    kalman_init(0.0625, 32, 1.3833094, 0);
}

void kalman_init(double _q, double _r, double _p, double intial_value)
{
    q = _q;
    r = _r;
    p = _p;
    x = intial_value;
}


double smoothing_add_sample(double measurement)
{
    p = p + q;
    k = p / (p + r);
    x = x + k * (measurement - x);
    p = (1 - k) * p;
    return x;
}

However, sometimes this gives me smoothing values 700(the normal range is 0-150) and then going down. I guess it happens when I initialize routine with 0 but immediately receiving reading above 0 (for example 40, 50).

How can I tweak those functions to naturally prevent such spikes, but still be able to smooth data.

Answer 1

The Kalman filter is an estimator, which minimizes the variance (p in your code) of a system state (x) of a linear system. The variance p can be interpreted as a measure of confidence in the value of x.

For each time step k the filter performs two steps:

1. The prediction step propagates the estimate one time step ahead (in your case according to x[k+1] = x[k] + w[k] where w[k] is zero-mean Gaussian noise with variance q). It means here that the state variance is increased by q.
2. The filtering step incorporates measurement information (according to measurement[k] = x[k] + v[k] where v[k] is zero-mean Gaussian noise with variance r).

For classic estimation problems p is initialized with a very large value (little confidence in the initial value x). Over time p decreases to a value somewhere around q. Note the p is independent of the measurement, so it only depends on q,r and p[0].

So for the tweaking:

• Initialize with a large p, i.e. p/r>>0; the initialization value of x does not really matter.
• The quotient r/q adjusts how strong the smoothing is (r/q=0 then there’s no smoothing at all).
• To choose the magnitude of r,q, you can assume Gaussian noise: Then you have a 95% probability the true value lies in the confidence interval of [x-2*sqrt(p), x+2*sqrt(p)].