Transcription
The idea of the unscented Kalman filter was developed by Simon Jolier in 2004. He said that if you have a nonlinear function, it's easier to approximate its distribution using some predefined deterministic points rather than approximating the linear version of that nonlinear function. You know, to write it down.
For example, assume you have a nonlinear transition function, something like this: f of xn plus some uncertainty. You have your measurement function as a function of your state plus some uncertainty. You cannot write this nonlinear function in a matrix form, or it's not easy to calculate it. Then we need something else to implement the Kalman filter. Julia said that if you have, for example, a state vector x with size L, we can define 2L + 1 auxiliary points, which we call sigma points, and propagate these sigma points into the transition and measurement function to calculate the mean and variance of the predicted states and predicted output.
But what are these sigma points? If I call them this, okay, assume for example, you write at the beginning of the Kalman filter process, n is equal to zero, and we have x0 and p0 as the true state or our prior knowledge. The sigma points 0 is x0, and sigma points 1 to n is equal to x0 plus the square root of L plus lambda. I call this one EI, PI equals 1 to n, and SPI is x0 minus the square root of L plus lambda PI. L is equal to n up to 2n + 1, and we have some weights corresponding to each of the sigma points, which we call them. For example, W0 is lambda / (L + lambda), WM is lambda / (2L + lambda), and here WIM is lambda / (2L + lambda). And I used M here. I will explain you later. And we have, in a special case, we have WC, which is equal to WI C, and WC is equal to W0, which is the same for M and C. Okay, I will come back to this weight later. But the thing is, you generate 2L + 1 predefined points in each iteration. Okay. And propagate these sigma points into the transition and measurement function to predict the mean and covariance or variance of the predicted states or predicted output.
Then, right now, I'm going to flash back to the main definition of the Kalman filter algorithm I had for you. Then, what we did, we calculated the mean and variance of the state vector, measured and predicted measurement, and used it to calculate the Kalman gain. Then, right now, we will do the same thing. Then, if I come back here, for example, if I write it down here for you. Now, instead of having one x and p to propagate, to propagate the x and p into the transition function, we have 2L + 1 points to propagate into the transition and measurement function. Then it will be something like this: for i = 1 up to 2L + 1. Okay. I just one thing I forgot to mention is this lambda, and this lambda we define it as a scaling factor, which I'll come back to later and show you how it affects these sigma points.
Then, for i = 1 to 2L + 1, we propagate each, for example, I call it X hat i, I don't know, i equals F of SPI, and following that, Y hat i is equal to H of the X hat i. Okay. Then here we will have 2L + 1 Xi and Yi. What we do after calculating these? We calculate the mean of this Xi, which I call it mean of Xi, which is the summation of i = 1 to 2n + 1, WIM, which I wrote here, Xi. And we do the same thing for. Okay. Then what I did here for 2n, 2L + 1 times, I calculated, let me do this for 2L + 1 times. I propagate the sigma points into the transition function and use the predicted state to predict the, to calculate the predicted output for each sigma point. And after that, I calculated the mean of the predicted output, predicted state, and predicted output. Then this is the mean, mean of the states, and the mean of the output, of course, predicted one.
Then, right now, we need to calculate the covariance of the states and predicted output. To calculate the, for example, I call it P hat X in this case, it will be equal to summation of i = 1 to 2L + 1, WIC, this X hat i minus the mean we calculated here, transpose. And we do the same thing for PY Y, Y i minus the calculated mean of the predicted output. Let's suppose you see what I did so far. I have 2L + 1 sigma points. I propagate each of the sigma points in the transition function. Predict the states for the next time. Use the predicted state to calculate the predicted output. Calculate the mean of the 2L + 1 predicted state and predicted output, and calculate the covariance of PXX and PYY. Okay.
Right now, what I have, I have the mean and covariance matrix of the predicted states and predicted output. This was step one. This was step two, calculating the mean and variance for the state and output. This was the covariance of the state and output. Now we have to do one thing more. We need to calculate the cross-covariance. I mean, P hat XY, which is equal to summation of i = 1 to L + 1, X i hat minus the mean of the X hat, and Y hat i minus the mean of the Y hat transpose. Since we want to calculate the Kalman gain, it will be equal to P hat XY, which is a cross-covariance, and this will be P hat YY plus R. This R is the measurement error, as we defined before. I mean, WN has a normal distribution with mean of zero and covariance of R. If you remember.
Then, using this Kalman gain, which will be updated in each iteration, we can find the true state Xn + 1 will be this mean at step n, for example. Yes. Plus Kalman gain, which we calculated. I put n to emphasize that it will be updated in each iteration. Y n + 1, which is the sensor data, minus this mean we calculated here for the sigma points. And what I did, I repeat again: the mean of the predicted state, its covariance, the mean of the predicted output, its covariance, Kalman gain, the mean of the true state or updated state, and its. It's right now we need this covariance. Covariance will be P at time n + 1, which is equal to P of the predicted states plus Q, incorporating the modeling error. Vn has a normal distribution with mean of zero and covariance of Q. Oh, you don't see it.
Then, yes. Then minus K, which is the Kalman gain, PY. We have it here, PYY. P hat Y Y plus R, and Kalman gain transpose. Then this was step three. This was step four. Step five, and these two are step six. Okay.
You see, the unscented Kalman filter follows the same procedure as the linear Kalman filter. But instead of working with one set of data points, which was the prior knowledge or the updated state and its covariance at the end of the iteration, we use the prior knowledge to generate 2L + 1 sets of data points, sigma points. We propagate each of the sigma points into the transition and measurement function to approximate the distribution of the predicted state. It increases the computational time and computational cost because instead of applying one time, you need to apply 2L + 1 times. But you don't need to go and calculate the derivative, which is sometimes impossible or very hard. You don't need to linearize the equation, and it's easy to implement. That's the most important thing for any type of nonlinear functions. You can just generate the points in each iteration and propagate it in the highly nonlinear system, which I'll show you later. And that's easy to implement. That's the main advantage. And in a couple of slides, I will show you how powerful it is.
Now, I told you that this lambda is the scaling factor. Then, similar to the unscented Kalman filter, we have another filter which we call the Cubature Kalman filter. It almost follows the same procedure as the unscented Kalman filter, but it uses another type of technique to calculate these predefined points, and they call it Cubature points. For example, I don't want to go inside, but here we have 2n + 1 points. In the Cubature Kalman filter, we have 2n points. For example, I'm showing you here. For example, you see the purple color is showing the UKF points, which includes the mean, the center point, which is the mean of the previous time or the prior knowledge. But for the Cubature, we don't take into account the mean point. We just look at the points around, and it has then what it means. In fact, we have fewer points, and the computational cost is a bit less than the unscented. If you use.
And one thing I want to mention here is the lambda, which I explained was a scaling factor. These scaling factors determine how much, how much the points spread around the mean point you have in the center. Then, if the lambda is a big value, the number, the auxiliary points around the center point will be spread wider or less. But if you use the Cubature Kalman filter, there's no scaling factor for that filter, and you just use some equations, and the location of the points always will be fixed. But in the unscented, as I said, we have this scaling factor, and this is somehow a tuning parameter which you need to tune if you want to implement the Kalman filter. And this is one of the things that you need to think about if you want to implement the unscented Kalman filter and find the optimal or the best value for this scaling factor.
Now, I've shown you the general overview of the unscented Kalman filter. I tried to explain the equations in a way to be understandable, but I didn't go into all the details where the equations come from. You know, I made, I wrote the simplified version of the sigma points and the corresponding weights. There are some special cases, some other hyperparameters. I didn't want to go into detail. You can find them in my papers and my thesis, which I put in the description. And right now, what I'm going to do, I'm going to show you one example and see how useful the unscented Kalman filter could be for parameter estimation of a highly nonlinear.