Your results will vary slightly depending on what numbers your random generator creates for the noise component of the noise, but the filter in the last section should track the actual position quite well. Typically as the filter starts up the first several predictions are quite bad, and varies a lot. But as the filter builds its state the estimates become much better.
Let’s start varying our parameters to see the effect of various changes. This is a very normal thing to be doing with Kalman filters. It is difficult, and often impossible to exactly model our sensors. An imperfect model means imperfect output from our filter. Engineers spend a lot of time tuning Kalman filters so that they perform well with real world sensors. We will spend time now to learn the effect of these changes. As you learn the effect of each change you will develop an intuition for how to design a Kalman filter. As I wrote earlier, designing a Kalman filter is as much art as science. The science is, roughly, designing the H and F matrices - they develop in an obvious manner based on the physics of the system we are modeling. The art comes in modeling the sensors and selecting appropriate values for the rest of our variables.
Let’s look at the effects of the noise parameters R and Q. We will want to see the effect of different settings for R and Q, so I have hard coded our measurements in zs based on a variance of 50 meters. That is very large, but it magnifies the effects of various design choices on the graphs, making it easier to recognize what is happening.
In [49]: # dog = DogSensor(velocity=1, noise_var=50) # zs = [dog.sense() for t in range(50)]
# I’ve hard coded zs to ensure we are all looking at the same data
zs = [3.59, 1.73, -2.575, 4.38, 9.71, 2.88, 10.08, 8.97, 3.74, 12.81, 11.15, 9.25, 3.93, 11.11, 19.29, 16.20, 19.63, 9.54, 26.27, 23.29, 25.18, 26.21, 17.1, 25.27, 26.86, 33.70, 25.92, 28.82, 32.13, 25.0, 38.56, 26.97, 22.49, 40.77, 32.95, 38.20, 40.93, 39.42, 35.49, 36.31, 31.56, 50.29, 40.20, 54.49, 50.38, 42.79, 37.89, 56.69, 41.47, 53.66] plot_track(data=zs, R_var=50, Q_var=20, count=50, plot_P=False,
title=’R_var = 50 m, Q_var = 20 m’)
plot_track(data=zs, R_var=50, Q_var=.02, count=50, plot_P=False, title=’R_var = 50 m, Q_var = 0.02 m’)
6.14. ADJUSTING THE FILTER 187
The filter in the first plot should follow the noisy measurement almost exactly. In the second plot the filter should vary from the measurement quite a bit, and be much closer to a straight line than in the first graph.
In the Kalman filter R is the measurement noise and Q is the process uncertainty. R is the same in both plots, so ignore it for the moment. Why does Q affect the plots this way?
Let’s remind ourselves of what the term process uncertainty means. Consider the problem of tracking a ball. We can accurately model its behavior in static air with math, but if there is any wind our model will diverge from reality.
In the first case we set Q var=20 m, which is quite large. In physical terms this is telling the filter “I don’t trust my motion prediction step” as we are saying that the variance in the velocity is 10. Strictly speaking, we are telling the filter there is a lot of external noise that we are not modeling with F, but the upshot of that is to not trust the motion prediction step. So the filter will be computing velocity ( ˙x), but then mostly
ignoring it because we are telling the filter that the computation is extremely suspect. Therefore the filter has nothing to use but the measurements, and thus it follows the measurements closely.
In the second case we set Q var=0.02 m, which is quite small. In physical terms we are telling the filter “trust the motion computation, it is really good!”. Again, more strictly this actually says there is very small amounts of process noise (variance 0.02 m), so the motion computation will be accurate. So the filter ends up ignoring some of the measurement as it jumps up and down, because the variation in the measurement does not match our trustworthy velocity prediction.
Now let’s set Q var=0.2 m, and bump R var up to 10,000 m. This is telling the filter that the measurement noise is very large.
In [50]: plot_track(data=zs, R_var=10000., Q_var=.2, count=50, plot_P=False, title=’R_var = 10000 m, Q_var = 0.2 m’)
The effect of this can be subtle. We have created an suboptimal filter because the actual measurement noise variance is 30 m, not 10,000 m. By setting the filter’s noise variance so high we force the filter to favor the prediction over the measurement. This can lead to apparently very smooth and good looking results. In the chart above the track may look extremely good to you since it follows the ideal path very closely. But, the ‘great’ behavior at the start should give you pause - the filter has not converged yet (P is still large) so it should not be able to be so close to the actual position. We can see that P has not converged because the entire chart is colored with the yellow background denoting the size of P. Let’s make R larger yet. Let’s see the result of a bad initial guess for the position by guessing the initial position to be 20 m and the initial velocity to be 1 m/s.
In [51]: plot_track(data=zs, R_var=10000., Q_var=.2, count=50, initial_x=np.array([20., 1.]), plot_P=False, title=’R_var = 10000 m, Q_var = 0.2 m’)
6.14. ADJUSTING THE FILTER 189
Here we can see that the filter cannot acquire the actual track at all. This happens because even though the filter is getting reasonable good measurements it assumes that the measurements are bad, and eventually just predicts forward from a bad position at each step. If you think that perhaps that bad initial position would give similar results for a smaller measurement noise, let’s set it back to the correct value of 50 m.
In [52]: plot_track(data=zs, R_var=30, Q_var=.2, count=50, initial_x=np.array([20., 1.]), plot_P=False, title=’R_var = 30 m, Q_var = 0.2 m’)
Here we see that the filter initially struggles for several iterations to acquire the track, but then it accurately tracks our dog. In fact, this is nearly optimum - we have not designed Q optimally, but R is optimal, and thus will yield optimal results. Recall that our rule of thumb for the variance of Q was to set it between 12∆a to ∆a, where ∆a is the maximum amount that the acceleration will change between sample
period. This only applies for the assumption we are making in this chapter - that acceleration is constant and uncorrelated between each time period. In the Kalman Math chapter we will discuss several different ways of designing Q.
To some extent you can get similar looking output by varying either R or Q, but I urge you to not ‘magically’ alter these until you get output that you like. Always think about the physical implications of these assignments, and vary R and/or Q based on your knowledge of the system you are filtering.