Kalman Filter C++

Hello folks,

So it’s yet another Kalman filter tutorial. My main source was this link and to be honest my implementation is quite exactly the same. But in C++.

I just though it would be good to actually give some explanation as to where this implementation comes from.

My goal was to filter a random 1D data from a laser scanner. I have measurement and they are extremely noisy, so I just want to reduce the noise.

So let’s dig in, here is the Kalman Filter class :

/*
* Need to tweak value of sensor and process noise
* arguments :
* process noise covariance
* measurement noise covariance
* estimation error covariance */

class Kalman_Filter_Distance{
protected :
double _q; //process noise covariance
double _q_init;
double _r; //measurement noise covariance
double _r_init;
double _x; //value
double _p; //estimation error covariance
double _p_init;
double _k; //kalman gain

public :
/*
* Need to tweak value of sensor and process noise
* arguments :
* process noise covariance
* measurement noise covariance
* estimation error covariance */
Kalman_Filter_Distance(double q, double r, double p) : _q(q), _q_init(q), _r(r), _r_init(0), _x(0), _p(p), _p_init(p), _k(_p / (_p + _r)){};
virtual void init(double x){ _x = x ;}
virtual void setProcessNoiseCovariance(double i){ _q = i; _q_init = i;}
virtual void setMeasurementNoiseCovariance(double i){_r = i; _r_init = i ;}
virtual void setEstimatiomErrorCovariance(double i){_p = i; _p_init = i ;}
virtual double kalmanUpdate(double measurement);
virtual void reset(){_q = _q_init; _r = _r_init ; _p = _p_init;};
};

double Kalman_Filter_Distance::kalmanUpdate(double measurement)
{
//prediction update
//omit _x = _x
_p = _p + _q;

//measurement update
_k = _p / (_p + _r);
_x = _x + _k * (measurement - _x);
_p = (1 - _k) * _p;

return _x;
}

Now, let’s break down the main function. I think the rest is pretty much self-explanatory.

//omit _x = _x
_p = _p + _q;

//measurement update
_k = _p / (_p + _r);
_x = _x + _k * (measurement - _x);
_p = (1 - _k) * _p;

return _x;

Discrete Kalman Filter

If you have been digging in the Kalman theory you must have encountered loooots of math stuff. The Kalman filter is made as such that :

time equation

{x_k}^{-} = A * x_{k - 1} + B * u_{k - 1}
{P_k}^{-} = A * P_{k - 1} * A^{T} + Q

measurement equation

K_k = {P_k}^{-} * H^{T} ( H * {P_k}^{-} * H^T + R )^{-1}
x_k = {x_k}^{-} + K_k ( z_k - H * {x_k}^{-} )
P_k = (I - K_k * H) * {P_k}^{-}

P_k is the estimate error.
K_k is the Kalman Gain.
R is the sensor noise
Q is the process noise
x_k is the filtered value
z_k is the measurement
A is the difference equation that relates the state at the previous time step k – 1
to the state at the current step k (in the absence of either a driving function or process noise). It’s “the system matrix”
B relates the optional control input u ∈ R to the state x. It’s “the control matrix”
H relates the state to the measurement zk. It’s the matrix that define the position of the tracked system depending on the measurement.

In my case, it’s pretty easy :

  • I have only one dimension since I’m only interested in filtering the value given by a single laser ray.
  • The matrix A is thus equal to A = [1].
  • I have no information about the driving forces. So B can directly be sent to 0 since u is also going to be 0.
  • H is the same as A since the system is the same as the observation. To see what is the utility of H look at this link.

If you replace all those data in the kalman equation, voilà :

time equation

{x_k}^{-} = x_{k - 1}
{P_k}^{-} = P_{k - 1} + Q

measurement equation

K_k = {P_k}^{-} ( {P_k}^{-} + R )^{-1}
x_k = {x_k}^{-} + K_k ( z_k - {x_k}^{-} )
P_k = (I - K_k) * {P_k}^{-}

You find exactly the equation in the code.

Playing with the variables

Now you need to play with the covariances to find a good fit to your usage.

For example here is my data unfiltered :

Non filtered data
Non filtered data

And here is the same data with q = 0.2, r = 32 , p = 1.

Filtered data
Filtered data

And again with q = 10, r = 1, p = 1

Filtered data 2
Filtered data 2

As you can see, and the second image, most a the noise part is gone. Although, the amplitude of the function is not as high as it was, so that’s something to take into account.

The third image shows that the value of q in my system is not very relevant, meaning noise doesn’t come from the process (which seems logical since I have no real process going on) while r is the important one (obviously observation noise).

As a side note, p is not very important since it is going to be updated in the process. Same for k.

Advertisements

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s