Help on function Q discrete white noise in module filterpy.common.discretization: Q discrete white noise(dim, dt=1.0, var=1.0)
Returns the Q matrix for the Discrete Constant White Noise
Model. dim may be either 2 or 3, dt is the time step, and sigma is the variance in the noise.
Q is computed as the G * G^T * variance, where G is the process noise per time step. In other words, G = [[.5dt^2][dt]]^T for the constant velocity model.
**Paramaeters** dim : int (2 or 3)
dimension for Q, where the final dimension is (dim x dim) dt : float, default=1.0
time step in whatever units your filter is using for time. i.e. the amount of time between innovations
var : float, default=1.0 variance in the noise
In [38]: Q = Q_discrete_white_noise(dim=2, dt=0.1, var=2.35) Q
Out[38]: array([[ 0.00005875, 0.001175 ], [ 0.001175 , 0.0235 ]])
6.10
Implementing the Kalman Filter
As we already explained, the Kalman filter equations are already implemented for you in the FilterPy library, so let’s start by importing it and creating a filter.
In [39]: import numpy as np
from filterpy.kalman import KalmanFilter dog_filter = KalmanFilter(dim_x=2, dim_z=1) help(KalmanFilter.__init__)
Help on function init in module filterpy.kalman.kalman filter: init (self, dim x, dim z, dim u=0)
Create a Kalman filter. You are responsible for setting the
various state variables to reasonable values; the defaults below will not give you a functional filter.
**Parameters** dim x : int
Number of state variables for the Kalman filter. For example, if you are tracking the position and velocity of an object in two dimensions, dim x would be 4.
This is used to set the default size of P, Q, and u dim z : int
Number of of measurement inputs. For example, if the sensor provides you with position in (x,y), dim z would be 2. dim u : int (optional)
size of the control input, if it is being used. Default value of 0 indicates it is not used.
That’s it. We import the filter, and create a filter that uses 2 state variables. We specify the number of state variables with the ‘dim=2’ expression (dim means dimensions).
The Kalman filter class contains a number of variables that you need to set. x is the state, F is the state transition function, and so on. Rather than talk about it, let’s just do it!
In [40]: dog_filter.x = np.array([0., 0.]) # state (location and velocity)
dog_filter.F = np.array([[1, 1], [0, 1]]) # state transition matrix
dog_filter.H = np.array([[1, 0]]) # Measurement function
dog_filter.R *= 5 # measurement noise
dog_filter.Q = Q_discrete_white_noise(2, dt=0.1, var=0.1) # process noise
dog_filter.P *= 500. # covariance matrix
Let’s look at this line by line.
1: We just assign the initial value for our state. Here we just initialize both the position and velocity to zero.
2: We set F =1 1 0 1
, as in design step 2 above. 3: We set H =1 0, as in design step 3 above.
4: We set R =5. R is initialized to the identity matrix, so multiplying by 5 performs this assignment for us.
5 We use the Q discrete white noise() method to set Q’s variance to 0.1.
5: P is initialized to the identity matrix of size n×n, so multiplying by 500 assigns a variance of 500 to x and ˙x. So f.P contains
500 0 0 500
All that is left is to run the code! The DogSensor class from the previous chapter has been placed in DogSensor.py.
In [41]: import numpy as np
from DogSensor import DogSensor
from filterpy.kalman import KalmanFilter def dog_tracking_filter(R, Q=0, cov=1.):
dog_filter = KalmanFilter(dim_x=2, dim_z=1)
dog_filter.x = np.array([0., 0.]) # (location and velocity)
dog_filter.F = np.array([[1, 1],
[0, 1]]) # state transition matrix
dog_filter.H = np.array([[1, 0]]) # Measurement function
dog_filter.R *= R # measurement uncertainty
dog_filter.P *= cov # covariance matrix
if np.isscalar(Q):
dog_filter.Q = Q_discrete_white_noise(dim=2, dt=0.1, var=Q) else:
6.10. IMPLEMENTING THE KALMAN FILTER 175
return dog_filter
def filter_dog(noise_var=0, count=0, R=0, Q=0, P=500., data=None, initial_x=None):
""" Kalman filter ’count’ readings from the DogSensor.
’noise_var’ is the variance of the noise from the measurements. ’data’ provides the measurements. If set, noise will
be ignored and data will not be generated for you. returns a tuple of (positions, measurements, covariance) """
if data is None:
dog = DogSensor(velocity=1, noise_var=noise_var) zs = [dog.sense() for t in range(count)]
else:
zs = data
dog_filter = dog_tracking_filter(R=R, Q=Q, cov=P) if initial_x is not None:
dog_filter.x = initial_x pos = [None] * count
cov = [None] * count for t in range(count):
z = zs[t]
pos[t] = dog_filter.x[0] cov[t] = dog_filter.P
# perform the kalman filter steps
dog_filter.update(z) dog_filter.predict() return (pos, zs, cov)
This is the complete code for the filter, and most of it is just boilerplate. The first function dog tracking filter() is a helper function that creates a KalmanFilter object with specified R, Q and P matrices. We’ve shown this code already, so I will not discuss it more here.
The function filter dog() implements the filter itself. Lets work through it line by line. The first line creates the simulation of the DogSensor, as we have seen in the previous chapter.
dog = DogSensor(velocity=1, noise=noise)
The next line uses our helper function to create a Kalman filter. dog_filter = dog_tracking_filter(R=R, Q=Q, cov=500.)
We will want to plot the filtered position, the measurements, and the covariance, so we will need to store them in lists. The next three lines initialize empty lists of length count in a pythonic way.
pos = [None] * count zs = [None] * count cov = [None] * count
Finally we get to the filter. All we need to do is perform the update and predict steps of the Kalman filter for each measurement. The KalmanFilter class provides the two functions update() and predict() for this purpose. update() performs the measurement update step of the Kalman filter, and so it takes a variable containing the sensor measurement.
Absent the bookkeeping work of storing the filter’s data, the for loop reads: for t in range (count):
z = dog.sense() dog_filter.update (z) dog_filter.predict()
It really cannot get much simpler than that. As we tackle more complicated problems this code will remain largely the same; all of the work goes into setting up the KalmanFilter variables; executing the filter is trivial.
Now let’s look at the result. Here is some code that calls filter track() and then plots the result. It is fairly uninteresting code, so I will not walk through it.
In [42]: import book_plots as bp
def plot_track(noise_var=None, count=0, R_var=0, Q_var=0, P=500., initial_x=None,data=None, plot_P=True, y_lim=None, title=’Kalman Filter’):
ps, zs, cov = filter_dog(noise_var=noise_var, data=data, count=count, R=R_var, Q=Q_var, P=P, initial_x=initial_x) actual = np.linspace(0, count-1, count)
cov = np.asarray(cov) std = np.sqrt(cov[:,0,0])
std_top = np.minimum(actual+std, [count+10]) std_btm = np.maximum(actual-std, [-50]) std_top = actual+std std_btm = actual-std bp.plot_track(actual) bp.plot_measurements(range(1, count+1), zs) bp.plot_filter(range(1, count+1), ps) plt.plot(actual+std, linestyle=’:’, c=’k’, lw=2) plt.plot(actual-std, linestyle=’:’, c=’k’, lw=2) plt.fill_between(range(count), std_btm, std_top, facecolor=’yellow’, alpha=0.2) plt.legend(loc=4)
if y_lim is not None: plt.ylim(y_lim) else: plt.ylim((-50, count+10)) plt.xlim((0,count)) plt.title(title) plt.show() if plot_P: ax = plt.subplot(121)
6.10. IMPLEMENTING THE KALMAN FILTER 177 ax.set_title("$\sigma^2_x$") plot_covariance(cov, (0, 0)) ax = plt.subplot(122) ax.set_title("$\sigma^2_y$") plot_covariance(cov, (1, 1)) plt.show()
def plot_covariance(P, index=(0, 0)): ps = []
for p in P:
ps.append(p[index[0], index[1]]) plt.plot(ps)
Finally, call it. We will start by filtering 50 measurements with a noise variance of 10 and a process variance of 0.01.
There is still a lot to learn, but we have implemented our first, full Kalman filter using the same theory and equations as published by Rudolf Kalman! Code very much like this runs inside of your GPS and phone, inside every airliner, inside of robots, and so on.
The first plot plots the output of the Kalman filter against the measurements and the actual position of our dog (drawn in green). After the initial settling in period the filter should track the dog’s position very closely. The yellow shaded portion between the black dotted lines shows 1 standard deviations of the filter’s variance, which I explain in the next paragraph.
The next two plots show the variance of x and of ˙x. If you look at the code, you will see that I have plotted the diagonals of P over time. Recall that the diagonal of a covariance matrix contains the variance of each state variable. So P[0, 0] is the variance of x, and P[1, 1] is the variance of ˙x. You can see that despite initializing P = (500 0
0 500) we quickly converge to small variances for both the position and velocity. The covariance matrix P tells us the theoretical performance of the filter assuming everything we tell it is true. Recall from the Gaussian chapter that the standard deviation is the square root of the variance, and that approximately 68% of a Gaussian distribution occurs within one standard deviation. Therefore, if at least 68% of the filter output is within one standard deviation we can be sure that the filter is performing well. In the top chart I have displayed the one standard deviation as the yellow shaded area between the two dotted lines. To my eye it looks like perhaps the filter is slightly exceeding that bounds, so the filter probably needs some tuning. We will discuss this later in the chapter.
In the previous chapter we filtered very noisy signals with much simpler code than the code above. However, realize that right now we are working with a very simple example - an object moving through 1-D space and one sensor. That is about the limit of what we can compute with the code in the last chapter. In contrast, we can implement very complicated, multidimensional filter with this code merely by altering are assignments to the filter’s variables. Perhaps we want to track 100 dimensions in financial models. Or we have an aircraft with a GPS, INS, TACAN, radar altimeter, baro altimeter, and airspeed indicator, and we want to integrate all those sensors into a model that predicts position, velocity, and accelerations in 3D (which requires 9 state variables). We can do that with the code in this chapter.