Visually Explained: Kalman Filters

You just graduated from college and Tesla just hired you after a few weeks on the job. You've just built your first self-driving car and you're ready to test it on the road, so you take it to the nearest highway to test it out. At time t equals zero, you launch the car from position x0 equals zero with a constant speed of 60 kilometers per hour. Now, being a good engineer, you want to track the position and speed of the car at regular intervals, say every hour. knowing where your car is in a perfect world tracking the car would be a piece of cake, the car's speed would be a constant 60 kilometers per hour and its position at time t plus one would simply be its position at time t plus delta speed times We usually like to write this kind of equations in matrix and vector format for convenience, so that the position and speed of the car evolve according to the simple linear dynamic system, if the last year of 2020 has anything to do with it, The word is actually far from perfect.

Your initial estimates of the car's speed and position are probably not perfectly accurate, for example, the speedometer you use may give you noisy speed measurements, check the manual provided by the car manufacturer. measuring instruments you use and find that the measurements you get are noisy and the noise can be modeled by some Gaussian variable with the following covariance matrix the way to interpret this zero sigma matrix is ​​that it represents your uncertainty about the state of the system the first diagonal element represents the uncertainty you have about the position of the car and the second diagonal element represents your uncertainty about the speed, the off-diagonal elements represent the correlation between the noise in the measurements with the speed and the position of the car.

Another problem you might face is that the conditions on the road are not perfect, especially if you live in California, which means that the nominal dynamic system that governs the movement of the car could be disturbed. We like to model these types of perturbations as Gaussian noise with a known covariance matrix that we call p all. At this point an hour has passed, what would be your best guess on where the car's position is right now and what is your guess on the car's speed? You might say: let me ignore all the imperfections in the world and follow this idealized dynamic system and see where it takes me.

The calculations in this case are quite simple. The car would remain at a constant speed of 60 kilometers per hour and after an hour it would be one kilometer away. from the initial position now, suddenly, you remember something that you actually have gps installed in the car and that gps can give you measurements of the position of the car every hour perfect, you check the gps data and it tells you that the car is 0.8 kilometers from the initial position, this GPS however does not measure the speed of the car, so now here is a question: which of these two measurements would you trust more, the one you have predicted ignoring all the imperfections of the word or the estimate What did you get from your measuring instruments?

However, can you combine both? estimates to get a new estimate that is more accurate than both estimates individually, for example, if I had to guess, I would say that the true position of the car is probably between the 0.8 and one kilometer mark and, as a consequence, the speed of the car should probably be less than 60 kilometers per hour, but how much less than 60 kilometers per hour exactly? And we are exactly in the 0.81 interval. Would you say the position of the car is right now? As you may have guessed, the answer probably depends on what we know about the size of the noise in our initial estimates in our measuring instruments in our model of the automotive dynamic system.

The answer to this question is what Kalman

filters

are all about, such as We will see, a Kalman filter is an algorithm to estimate the state of the system, for example the speed and position of the car using past and possibly noisy observations and current and possibly noisy measurements of that system. More specifically, let's say this is the underlying dynamical system and let's say this is the measurement that we get at every time t we get a measurement there is a random variable yt, so you can see that yt is a linear function of the state xt plus something of noise b which we model as a random Gaussian variable with a known covariance matrix q in the toe example.

I just saw h was row vector 1 0 because the only thing we measure via gps is position and that was the first component of our state vector. Kelman xt filtering is a two-step process. Is that how it works. The first step is called. In the prediction step, in this step you take any previous estimates you had about the state of the system, run these estimates through the idealized version of the dynamical system, and that would be your prediction for the state of the system in the next step i'. I'm going to mark these estimates with a minus sign at the top.

The reason I do this is because this will not be our final estimate since we have not yet incorporated the information we get from our measuring instruments. Another important thing we have to do in this prediction step is to think about how our noise covariance matrix propagates through the system in the case that we have perfect estimates of our state variables, that is, when sigma t is zero, the only uncertainty will come from the perturbation of our dynamical system, that is, from the p matrix in general, although sigma t would not be zero, so when you run this sigma t through the idealized system and do the calculations, you realize that we need to add an additional term, a sigma a, transposed, so what does this mean for our toy?

For example, in the prediction step we simply predicted that the car would be at this yellow dot, so now the question is how can I incorporate the additional information I get from my measuring instruments to update and improve this prediction that is made in the second Kalman filtering step, which is appropriately called the update step, in this step we take the results of our prediction in terms of the state of the system and the noise covariance matrix and take the new measurements that we made on the system that are noisy, remember. and we combine both into a single estimate, which would be our final estimate of the system.

So how does this work? What we need to calculate is the surprise term. The surprise term is the difference between what you predicted and what you measured, the greater the difference. The term is the more you are surprised and the more you question everything you learned in college. Now you look at this image and you need to decide how much you need to correct your prediction based on the surprise term and for that you need to see. how much you trust your prediction, which is measured by this covariance matrix, and how much you trust your measuring instruments, which is measured by this covariance matrix, more precisely, you need to calculate this ratio, sometimes called the Kalman ratio, the sigma term is your uncertainty about your prediction for the position and this q term is your uncertainty about the GPS measurements, this ratio is always between -1 and 1.

If it is 0, it means that the sigma term is 0, which means that He has complete faith in the predictions he made. You are basically saying that my car is at the yellow dot, on the other hand, if it is 1, it means that the q term is 0, which means that you completely trust your GPS and you are saying that the car is at the blue dot in the practice. although this ratio will be between 0 and 1. So what you need to do is take your prediction and correct it with the k term multiplied by the delta x surprise term, the larger the ratio the larger the correction, this is how we correct our prediction for the position, but now how do we correct our prediction for the car's speed?

Remember that our GPS doesn't measure speed at all, so we need to somehow use what we know about another state variable, position, to infer how to correct our prediction. for speed and this is the magic of Kalman

filters

, you don't need to measure everything all the time, partial measurements are still fine, so what is the correct Kalman coefficient to use? Here you see a common coefficient that is very similar to the one we had. before, except now the numerator is replaced by the off-diagonal term sigma x x point which represents the correlation between velocity and position and this makes sense.

This is the term you want to use to convert a position information into any velocity information in the worst case, if the noise in velocity and position are independent of each other, this ratio will be zero, which means that what we know about the position of the car would be completely useless as far as speed is concerned and there is an identical formula. To correct our velocity prediction more generally, going back to the general formula, you get the following equations to correct your predictions for the system state and the notes covariance matrix. There are two things I'm not doing here.

I don't tell you how to differentiate these two expressions in detail and I don't tell you how to calculate this common term k, which is a matrix. In the most general case, there are countless references online, including Wikipedia, for that and I think you're in a great position to follow what's going on there if you've understood the intuition of Kalman filters

explained

in this video thank you very much and until next time