Kalman Filter - Implementation, Parameters and Tuning

Question

First of all, this is the first time I try to make a Kalman filter.

I earlier posted the follwoing question Filter out noise and variations from speed values on StackOverflow which describes the background for this post. This is a typical sample of values I'm trying to filter. They don't necessarily have to decrease which is the case here. But the rate of change is typically like this

X------- Y
16 ---233.75
24 ---234.01
26 ---234.33
32 ---234.12
36 ---233.85
39 ---233.42
47 ---233.69
52 ---233.68
55 ---233.76
60 ---232.97
66 ---233.31
72 ---233.99

I have implemented my Kalman Filter according to this tutorial: Kalman Filter for Dummies.

My implementation looks like this (pseudocode).

//Standard deviation is 0.05. Used in calculation of Kalman gain

void updateAngle(double lastAngle){
  if(firsTimeRunning==true)
     priorEstimate = 0;               //estimate is the old one here
     priorErrorVariance = 1.2;        //errorCovariance is the old one
  else
     priorEstimate = estimate;              //estimate is the old one here
     priorErrorVariance = errorCovariance;  //errorCovariance is the old one
  rawValue = lastAngle;          //lastAngle is the newest Y-value recieved
  kalmanGain = priorErrorVariance / (priorErrVariance + 0.05);
  estimate = priorEstimate + (kalmanGain * (rawValue - priorEstimate));
  errorCovariance = (1 - kalmanGain) * priorErrVariance;
  angle = estimate;              //angle is the variable I want to update
}                                //which will be lastAngle next time

I start out with a prior estimate of 0. This seems to work fine. But what i notice is that the kalmanGain will decrease each time this update is run, which means that I trust my new values less the longer my filter is running (?). I don't want that.

I went from just using a moving average (simple and exponential weighted) to using this. Right now I can't even get as good results as that did.

My question is if this is the right implementation and if my prior error variance and standard deviation looks good according to the sample values I have posted? My parameters are actually just picked randomly to see if I could get some good results. I have tried several different ranges but with poor results. If you have any suggestions to changes I can do, it would be really appreciated. I'm sorry if there is some obvious things missing. First time posting here too.

Answer 1

Kalman filters are useful when your input signal consists of noisy observations of some linear dynamical system's state. Given a series of observations of the system state, the Kalman filter aims to recursively provide better and better estimates of the underlying system's state. In order to apply it successfully, you need to have a model for the dynamics of the system whose state you're estimating. As described in detail at Wikipedia, this model describes how the state underlying system is expected to change over one time step, given its previous state, any inputs to the system, and a Gaussian-distributed stochastic component called process noise.

With that said, it's not clear from your question whether you have any such underlying model. The linked post indicated that you're consuming speed values from a sensor. These can be modeled as direct observations of a system's state (where the state is its speed), or indirect observations of its state (where the state is its position, for example). But, in order to use the Kalman framework, you need to choose a model for how that state is expected to evolve as time goes on; this additional information is used in order to generate the optimal estimate. A Kalman filter isn't a magic black box that will just "clean up" a signal that is applied to it.

With that said, the phenomenon that you alluded to, where the Kalman filter will become increasingly confident in its own output to the point where input observations become progressively ignored, does happen in practice. This can be mitigated by manually increasing the values in the process noise covariance matrix. Then, qualitatively, the model for the system's state transition contains a larger stochastic component, so the ability for the estimator to accurately predict the next state given the current state is diminished. This will reduce its reliance on its current estimate of the system state and increase its reliance upon subsequent observations, preventing the "ignoring the input" effect.

Answer 2

If I understood it correctly, you have something that is moving and you can observe the speed and this speed is noisy. From your measurements, you observe 2 kinds of variations.\

1. Variations caused by the noise
2. Variations because the object is truly changing the speed (e.g. turning)

The reason your Kalman gain goes to zero is that you have implicitly assumed that the speed of the object is constant and all you need to do is to estimate this true speed.

"Hey, I have an object that is moving at a constant speed and I want to estimate this constant speed"

Your model is like this, where $x_k$ is the speed at time $k$ and $y_k$ is the corresponding measurement.

$$ x_k = x_{k-1} $$ $$ y_k = x_k + q_k $$

But your object isn't moving that way. It's speed is changing and you don't know how and when it's going to change.

What you have to say instead is:

"Hey, I have an object that is moving at a speed but I'm not sure about how it's changing its speed"

There are many ways you can do this: The simplest way is to add uncertainty your state.

$$x_{k} = x_{k-1} + \underbrace{v_{k-1}}_{\text{you add uncertainty}}$$ $$ y_k = x_k + q_k $$ where $q_k$ and $v_k$ are assumed to be white noise.

Your Kalman Filter equations will look like this:

$$ \hat y_{k|k-1} = \hat x_{k|k-1} $$ $$ K_{k} = \frac{P_{k|k-1}}{P_{k|k-1} + Q^o}$$ $$ \hat x_{k|k}= \hat x_{k|k-1} + K_k(y_k - \hat y_{k|k-1})$$ $$ P_{k|k} = P_{k|k-1} - K_kP_{k|k-1} $$ $$ P_{k+1|k} =P_{k|k} + Q^s $$

In your case the 0.05 value is the Observation noise covariance, $Q^o$. To make this change all you have to do is set $Q^s$, the state noise covarinace (the uncertanity in your state) to some constant value.

In your code the slight modification would be:

stateVariance = 0.5

errorCovariance = (1 - kalmanGain) * priorErrVariance + stateVariance;

By not adding the stateVariance or $Q^s$ in your code you assumed that it is zero.

This stateVariance value can be anything you want. It's based on your confidence on how much the speed will actually change. If you think the speed will remain fairly constant, set this to a small number.

This way your Kalman Gain will not go to zero.

Answer 3

You need a dynamic system to use a Kalman Filter.

I would suggest

$$y = \sum\limits_{i =0}^n a_i\, x^i $$

$$a[k+1] = a[k] + w $$ $$ cov(w) = Q$$ Measurement: $$z = \sum\limits_{i =0}^n a_i\, x^i = y$$

So instead of using $x$ as states, introduce the coefficient ($a$) as states

Answer 4

I think you could use some ideas from classical control theory, e.g. PID controller.

Your signal Y can be the setpoint of the controller u(t). The process plant is just 1 and y(t) will be filtered output. All you'll have to do is to set the parameters (tune) P,I and D to get what you want.

The output y(t) will try to "follow" the input u(t), but the parameters control how this tracking will be.

The differential gain D will make your response sensitive to rapid error changes. In your case, I think D should be small. You don't want y(t) to change if u(t) abruptly changes.

The integral gain 'I' will make your response sensitive to accumulated error. You should put a high value there. If u(t) changes level and keeps it there, the error will build up and then you want y(t) to do the same.

The P gain can give a fine tune. Anyway, try to play with the parameters and see what you get.

There are complicated tuning methods, though, but I don't believe you'll need it.

Good luck.

Answer 5

Here is a simple and clean implementation of Kalman Filter that follows notations as in Wikipedia page. https://github.com/zziz/kalman-filter

Answer 6

Here is an algorithm that does the same thing but slightly better when I tested it (smaller phaze shift). It is also simpler to understand. good luck :) https://github.com/Wolfiwolf/my_signal_filter