Modified 2018-10-16 by Josh Roy
Modified 2018-10-15 by Theo Guerin
So far, we have discussed the standard Kalman Filter algorithm. However, we have not mentioned its limitations. The standard Kalman Filter assumes that the system is both linear and Gaussian. In other words, the uncertainties in the motion and measurement models are assumed to be normally distributed about a mean in order to produce optimal estimates, which allows us to represent the state estimate as a Gaussian with mean and variance. For many systems, the Gaussian assumption is a good one. Intuitively, one can imagine that a sensor’s noise, for example, varies more or less symmetrically about a true mean value, with larger deviations occurring less frequently.
The greater constraint, however, is the assumption that the system is linear. What we mean by this is that the state transition function and measurement function are linear functions, and as a result, when we pass Gaussian distributions through these functions, the output remains Gaussian or proportional to a Gaussian. An arbitrary nonlinear function, on the other hand, will not output another Gaussian or scaled Gaussian, which is a problem since so much of the Kalman Filter math depends on the state estimate being Gaussian. The Unscented Kalman Filter was expressly designed to robustly handle this issue of nonlinearity.
In this project’s -axis UKF, the functions are linear, so indeed a standard Kalman Filter would suffice. However, for the second UKF that you will be implementing, there are nonlinearities due to the drone’s orientation in space. To make the transition easier from the first part to the second part of this project, we are asking you to implement a UKF even for a linear system. The UKF estimates will be the same as a KF; the only downsides might be code complexity and computation time. That said, you will be using a Python library called FilterPy (written by Labbe, author of Kalman and Bayesian Filters in Python [4]) that handles and hides most of the filtering math anyway.
You might also be wondering what the term “unscented” has to do with a Kalman Filter that applies to nonlinear systems. There is no greater technical meaning to the word; the inventor claims it is an arbitrary choice that resulted from his catching a glimpse of a coworker’s deodorant while trying to come up with a name for his filter [16].
Modified 2018-10-16 by Theo Guerin
To handle the nonlinearities, the UKF uses a sampling scheme. An alternative to the UKF known as the Extended Kalman Filter (EKF) uses Jacobians to linearize the nonlinear equations, but the UKF takes a deterministic sampling approach that in many cases results in more accurate estimates and is a simpler algorithm to implement [9].
The UKF uses a function to compute so-called sigma points, which are the sample points to pass through the state transition and measurement functions. Each sigma point also has corresponding weights for the sample’s mean and covariance. The sigma points are generated so that there are of them, where is the size of the state vector. Imagine a one-dimensional state vector, for example, which we represent as a single-variable Gaussian. In this instance, sigma points are chosen. One of these points is the mean of the Gaussian, and the two other points are symmetric about the mean on either side. The exact distance of these points from the mean sigma point will vary depending on parameters passed into the sigma point function, but we do not expect you to worry about these parameters. The idea, though, is that these sigma points and their weights are sufficiently representative of the Gaussian distribution.
Next, these points that represent the Gaussian state estimate are passed through a nonlinear function (i.e., the state transition or measurement functions), which can scatter the points arbitrarily. We then want to recover a Gaussian from these scattered points, and we do so by using the unscented transform, which computes a new mean and covariance matrix. To compute the new mean, the unscented transform calculates a weighted sum of each sigma point with its associated sample mean weight.
Modified 2018-10-15 by Theo Guerin
The UKF formulates the prior state estimate by specifying a set of sigma points according to the current state estimate and then propagating these points through the state transition function to yield a new set of sigma points , which are passed through the unscented transform to produce the prior state estimate.
Modified 2018-10-16 by Theo Guerin
Below is the algorithm for the Unscented Kalman Filter [9] [4]. Note that the sigma point weights denoted by and can be computed as part of a number of sigma point algorithms. We will use Van der Merwe’s scaled sigma point algorithm to compute the sigma points and weights [20] [4]. The sigma points get computed at each prediction, whereas the weights can be computed just once upon filter initialization.