The Unscented Kalman Filter

The UKF addresses the approximation issues of the EKF. The state distribution is again represented by a GRV, but is now specified using a minimal set of carefully chosen sample points. These sample points completely capture the true mean and covariance of the GRV, and when propagated through the true non-linear system, captures the posterior mean and covariance accurately to the 3rd order (Taylor series expansion) for any nonlinearity. To elaborate on this, we start by first explaining the unscented transformation.

The unscented transformation (UT) is a method for calculating the statistics of a random variable which undergoes a nonlinear transformation [1]. Consider propagating a random variable ${\bf x}$ (dimension $L$) through a nonlinear function, ${\bf y} = g({\bf x})$. Assume ${\bf x}$ has mean $\bar{\bf x}$ and covariance ${\bf P}_{\bf x}$. To calculate the statistics of ${\bf y}$, we form a matrix $\boldsymbol{\mathcal{X}}$ of $2L+1$ sigma vectors ${\mathcal{X}}_i$ (with corresponding weights $W_i$), according to the following:

$$\mathcal{X}_0 = \bar{\bf x}$$ (15)

$$\mathcal{X}_i = \bar{\bf x} + \left(\sqrt{(L+\lambda)\mathbf{P}_{\bf x}}\right)_i i=1,\ldots,L$$

$$\mathcal{X}_i = \bar{\bf x} - \left(\sqrt{(L+\lambda)\mathbf{P}_{\bf x}}\right)_{i-L} i=L+1,\ldots,2L$$

$$W_0^{(m)} = \lambda/(L+\lambda)$$

$$W_0^{(c)} = \lambda/(L+\lambda) + (1-\alpha^2+\beta)$$

$$W_i^{(m)} = W_i^{(c)}=1/\{2(L+\lambda)\} i=1,\ldots,2L$$

where $\lambda=\alpha^2(L+ {\kappa})-L$ is a scaling parameter. $\alpha$ determines the spread of the sigma points around $\bar{\bf x}$ and is usually set to a small positive value ( e.g., 1e-3). $\kappa$ is a secondary scaling parameter which is usually set to 0, and $\beta$ is used to incorporate prior knowledge of the distribution of ${\bf x}$ (for Gaussian distributions, $\beta=2$ is optimal). $(\sqrt{(L+\lambda)\mathbf{P}_{ {\bf x}}})_i$ is the $i$th row of the matrix square root. These sigma vectors are propagated through the nonlinear function,

$${\mathcal{Y}}_i = g({\mathcal{X}}_i) \quad i=0,\ldots,2L \quad,$$ (16)

and the mean and covariance for ${\bf y}$ are approximated using a weighted sample mean and covariance of the posterior sigma points,

$${\bar{\bf y}} \approx \sum_{i=0}^{2L} W_i^{(m)}{\mathcal{Y}}_i$$ (17)

$${\mathbf{P}}_{ {\bf y}} \approx \sum_{i=0}^{2L} W_i^{(c)}\left\{{\mathcal{Y}}_i-\bar{\bf y}\right\}\left\{{\mathcal{Y}}_i-\bar{\bf y}\right\}^T$$ (18)

Note that this method differs substantially from general ``sampling'' methods ( e.g., Monte-Carlo methods such as particle filters [7]) which require orders of magnitude more sample points in an attempt to propagate an accurate (possibly non-Gaussian) distribution of the state. The deceptively simple approach taken with the UT results in approximations that are accurate to the third order for Gaussian inputs for all nonlinearities. For non-Gaussian inputs, approximations are accurate to at least the second-order, with the accuracy of third and higher order moments determined by the choice of $\alpha$ and $\beta$ (See [8] for a detailed discussion of the UT).

A simple example is shown in Figure 1 for a 2-dimensional system: the left plot shows the true mean and covariance propagation using Monte-Carlo sampling; the center plots show the results using a linearization approach as would be done in the EKF; the right plots show the performance of the UT (note only 5 sigma points are required). The superior performance of the UT is clear. The Unscented Kalman Filter (UKF) is a straightforward extension of the UT to the recursive estimation in Equation 8, where the state RV is redefined as the concatenation of the original state and noise variables: ${\bf x}_k^a = [ {\bf x}^T_k \ \mathbf{v}^T_k \\mathbf{n}^T_k]^T$. The UT sigma point selection scheme (Equation 15) is applied to this new augmented state RV to calculate the corresponding sigma matrix, $\boldsymbol{\mathcal{X}}^a_k$. The UKF equations are given in Algorithm 3. Note that no explicit calculation of Jacobians or Hessians are necessary to implement this algorithm. Furthermore, the overall number of computations are the same order as the EKF.