Derivation of Unscented Kalman Filter (UKF)

Linearization of non-linear dynamics/observation in the EKF is a clever trick to use the KF equations. We approximate a new linear function from non-linear function around one point i.e. the mean of the Gaussian. However, there is still a better way to approximate it, say we approximate around multiple points. This is the inituition behind the powerful method called Unscented Kalman Filter (UKF). We take some points on source Gaussian and map them on target Gaussian after passing points through some non linear function and then we calculate the new mean and variance of transformed Gaussian. These special set of points are called "Sigma points". It can be very difficult to transform whole state distribution (computationally expensive) through a non linear function but it is very easy to transform some individual points. These sigma points are the representatives of whole distribution.

Unscented Transform

Consider a non-linear function $z=f(x)$ for a random variable ${\mathbb{R}}^n\ni x\sim N(\mu ,\mathrm{\Sigma })$. Then the sigma points are computed as, $$\begin{array}{rl}{x}^{(i)}& =\mu +{\sqrt{n{\mathrm{\Sigma }}_i}}^T\\ x^{(n+i)}& =\mu -{\sqrt{n{\mathrm{\Sigma }}_i}}^T\\ \text{(1)}& \text{for }& i=1,...,n\end{array}$$ where $\sqrt{n{\mathrm{\Sigma }}_i}$ is the $i^{th}$ row of the matrix $\sqrt{n\mathrm{\Sigma }}$. The matrix square root is computed using the Cholesky decomposition - if we can decompose $\mathrm{\Sigma }=S{S}^T$, then $S=\sqrt{\mathrm{\Sigma }}$. There are $2n$ sigma points for an $n$-dimensional Gaussian. Each sigma point is assigned an equal weight, $$\begin{array}{}\text{(2)}& w^{(i)}=\frac{1}{2n}\end{array}$$ Each sigma point is then transformed with a non-linear function $f$ to get the transformed sigma points $z^{(i)}=f(x^{(i)})$. The mean and covariance of the transformed points can be computed as, $$\begin{array}{}\text{(3)}& {\mu }_z& =\sum _{i=1}^{2n}{w}^{(i)}{z}^{(i)}=\sum _{i=1}^{2n}{w}^{(i)}f(x^{(i)})\text{(4)}& {\mathrm{\Sigma }}_z& =\sum _{i=1}^{2n}{w}^{(i)}(z^{(i)}-{\mu }_z)(z^{(i)}-{\mu }_z{)}^T=\sum _{i=1}^{2n}{w}^{(i)}(f(x^{(i)})-{\mu }_z)(f(x^{(i)})-{\mu }_z{)}^T\end{array}$$
The Unscented Transform gives us a way to accurately estimate the mean and covariance of the transformed distribution through a nonlinearity. We can use the UT to modify the EKF to make it a more accurate state estimator. The resultant algorithm is called the Unscented Kalman Filter (UKF). The UT can aslo be modified in varied ways through the inclusion of mean as one of the sigma points and the scaling factor.

Consider a non-linear dynamical system with non-linear observations, $$\begin{array}{rl}{x}_{k+1}& =f(x_k,u_k)+ϵ\\ y_k& =g(x_k)+\upsilon \end{array}$$ where noise vectors $ϵ\sim N(0,R)$ and $\upsilon \sim N(0,Q)$ are defined similar to Part 3.

Step 1: Propagating the dynamics by one timestep

Given our current state estimate ${\mu }_{k|k}$ and ${\mathrm{\Sigma }}_{k|k}$, we use the UT to obtain the updated estimates ${\mu }_{k+1|k}$ and ${\mathrm{\Sigma }}_{k+1|k}$. If $x^{(i)}$ are the sigma points of the Guassian $N({\mu }_{k|k},{\mathrm{\Sigma }}_{k|k})$ and $w^{(i)}$ are the corresponding weights, we transform them using the non-linear function and obtain new mean and covariance (using Equation 3 and 4) as, $$\begin{array}{rl}\text{(5)}& {\mu }_{k+1|k}& =E[{\hat{x}}_{k+1|k}]=E[f({\hat{x}}_{k|k},u_k)+ϵ]=\sum _{i=1}^{2n}{w}^{(i)}f(x^{(i)},u_k){\mathrm{\Sigma }}_{k+1|k}& =\text{cov}({\hat{x}}_{k+1|k})=\text{cov}(f({\hat{x}}_{k|k},u_k)+ϵ)\\ \text{(6)}& & =\sum _{i=1}^{2n}{w}^{(i)}(f(x^{(i)})-{\mu }_{k+1|k})(f(x^{(i)})-{\mu }_{k+1|k}{)}^T+R\end{array}$$

Computing general form of Kalman gain

The observation step is also modified using the UT. We need a way to compute the Kalman gain in terms of the sigma points in the UT. Until now we have written the Kalman gain using the measurement matrix $C$. We will now discuss a more abstract formulation that gives the same expression.

Let's get back to our standard Kalman Filter equations (Step 2 of Part 2) to obtain a general form of the kalman gain and update equations, i.e. without the measurement matrix $C$. Recall that the linear observation model was given as $y_{k+1}=C{x}_{k+1|k}+\upsilon$ where $\upsilon \sim N(0,Q)$. An estimate of this observation (or prediction of the measurement) is given as, $$\begin{array}{}\text{(7)}& {\hat{y}}_{k+1}=C{\hat{x}}_{k+1|k}+\upsilon \end{array}$$ From Step 1, we obtain ${\mu }_{k+1|k},{\mathrm{\Sigma }}_{k+1|k}$ and we want to incorporte a new observation $y_{k+1}$. We define the joint distribution of the previous state estimate ${\hat{x}}_{k+1|k}$ and observation estimate ${\hat{y}}_{k+1}$ as, $$p=\left[\begin{array}{c}x\\ y\end{array}\right]\sim N(\left[\begin{array}{c}{\mu }_x\\ {\mu }_y\end{array}\right],\left[\begin{array}{cc}{\mathrm{\Sigma }}_{xx}& {\mathrm{\Sigma }}_{xy}\\ {\mathrm{\Sigma }}_{yx}& {\mathrm{\Sigma }}_{yy}\end{array}\right])$$ For simpler notation we use ${\hat{x}}_{k+1|k}=x$ and ${\hat{y}}_{k+1}=y$. We already have $$\begin{array}{rl}{\mu }_x& =E[{\hat{x}}_{k+1|k}]={\mu }_{k+1|k}\\ {\mathrm{\Sigma }}_{xx}& =\text{cov}({\hat{x}}_{k+1|k})={\mathrm{\Sigma }}_{k+1|k}\end{array}$$ The mean of the estimate of the observation is ${\mu }_y=E[{\hat{y}}_{k+1}]$. For the linear system, the covariances can be obtained as, $$\begin{array}{rl}\text{(8.1)}& {\mathrm{\Sigma }}_{yy}& =\text{cov}({\hat{y}}_{k+1})=\text{cov}(C{\hat{x}}_{k+1|k}+\upsilon )=C{\mathrm{\Sigma }}_{k+1|k}{C}^T+Q{\mathrm{\Sigma }}_{xy}& =E[({\hat{x}}_{k+1|k}-E[{\hat{x}}_{k+1|k}])({\hat{y}}_{k+1}-E[{\hat{y}}_{k+1}]{)}^T]\\ & =E[({\hat{x}}_{k+1|k}-{\mu }_{k+1|k})(C{\hat{x}}_{k+1|k}+\upsilon -E[C{\hat{x}}_{k+1|k}+\upsilon ]{)}^T]\\ & =E[({\hat{x}}_{k+1|k}-{\mu }_{k+1|k})(C{\hat{x}}_{k+1|k}+\upsilon -C{\mu }_{k+1|k}{)}^T]\\ & =E[({\hat{x}}_{k+1|k}-{\mu }_{k+1|k})(C({\hat{x}}_{k+1|k}-{\mu }_{k+1|k})+\upsilon {)}^T]\\ & =E[({\hat{x}}_{k+1|k}-{\mu }_{k+1|k})(({\hat{x}}_{k+1|k}-{\mu }_{k+1|k}{)}^T{C}^T+{\upsilon }^T)]\\ & =E[({\hat{x}}_{k+1|k}-{\mu }_{k+1|k})({\hat{x}}_{k+1|k}-{\mu }_{k+1|k}{)}^T]C^T+E[({\hat{x}}_{k+1|k}-{\mu }_{k+1|k})]E[{\upsilon }^T]\\ \text{(8.2)}& & ={\mathrm{\Sigma }}_{k+1|k}{C}^T\text{(8.3)}& {\mathrm{\Sigma }}_{yx}& ={\mathrm{\Sigma }}_{xy}^T=({\mathrm{\Sigma }}_{k+1|k}{C}^T{)}^T=C{\mathrm{\Sigma }}_{k+1|k}\end{array}$$ We can write the Kalman gain equation (equation 4 of Part 2) in terms of the covariances (we do this to get rid of the matrix $C$) using Equations 8.1,8.2 as, $$\begin{array}{rl}{K}_{k+1}& ={\mathrm{\Sigma }}_{k+1|k}{C}^T(C{\mathrm{\Sigma }}_{k+1|k}{C}^T+Q{)}^{-1}\\ \text{(9)}& K_{k+1}^{\ast }& ={\mathrm{\Sigma }}_{xy}({\mathrm{\Sigma }}_{yy}{)}^{-1}\end{array}$$ $K^{\ast }$ is the representation of Kalman gain in a general form which is applicable for all types of systems. The updated estimate (Equation 5 of Part 2) can be written through rearrangement of Equation 7 as, $$\begin{array}{rl}{\hat{x}}_{k+1|k+1}& ={\hat{x}}_{k+1|k}+K_{k+1}(y_{k+1}-C{\hat{x}}_{k+1|k})\\ & ={\hat{x}}_{k+1|k}+K_{k+1}^{\ast }(y_{k+1}-{\hat{y}}_{k+1}+\upsilon )\end{array}$$ The mean of this new estimator can be computed as, $$\begin{array}{rl}{\mu }_{k+1|k+1}& =E[{\hat{x}}_{k+1|k+1}]=E[{\hat{x}}_{k+1|k}+K_{k+1}^{\ast }(y_{k+1}-{\hat{y}}_{k+1}+\upsilon )]\\ \text{(10)}& & ={\mu }_{k+1|k}+K_{k+1}^{\ast }(y_{k+1}-{\mu }_y)\end{array}$$ And the covariance of this estimator (Equation 6.2 of Part 2) can be written in a similar fashion using Equations 8.1,8.2,8.3 as, $$\begin{array}{rl}{\mathrm{\Sigma }}_{k+1|k+1}& =(I-K_{k+1}C){\mathrm{\Sigma }}_{k+1|k}={\mathrm{\Sigma }}_{k+1|k}-K_{k+1}C{\mathrm{\Sigma }}_{k+1|k}\\ & ={\mathrm{\Sigma }}_{k+1|k}-{\mathrm{\Sigma }}_{k+1|k}{C}^T(C{\mathrm{\Sigma }}_{k+1|k}{C}^T+Q{)}^{-1}C{\mathrm{\Sigma }}_{k+1|k}\\ \text{(11.1)}& & ={\mathrm{\Sigma }}_{k+1|k}-{\mathrm{\Sigma }}_{xy}({\mathrm{\Sigma }}_{yy}{)}^{-1}{\mathrm{\Sigma }}_{yx}\text{(11.2)}& & ={\mathrm{\Sigma }}_{k+1|k}-K^{\ast }{\mathrm{\Sigma }}_{yy}{K}^{\ast T}\end{array}$$

Step 2: Incorporating the observation

It is clear from the above derivation that we need the estimate of the new observation to proceed further. We generate new sigma points $x^{(i)}$ on the updated distribution $N({\mu }_{k+1|k},{\mathrm{\Sigma }}_{k+1,k})$ obtained from Step 1 with weights $w^{(i)}$. We then transform them using the non-linear observation model $g$. These transformed points estimate our new observation $y_{k+1}$. To apply equations 9, 10 and 11, we need ${\mu }_y,{\mathrm{\Sigma }}_{yy},{\mathrm{\Sigma }}_{xy}$. We compute them as follows,

The mean of the transformed sigma points is nothing but the mean of the estimation of the observation ${\mu }_y$. We use Equation 3 to obtain, $$\begin{array}{}\text{(12)}& {\mu }_y=E[{\hat{y}}_{k+1}]=E[g({\hat{x}}_{k+1|k})+\upsilon ]=\sum _{i=1}^{2n}{w}^{(i)}g(x^{(i)})\end{array}$$ The other covariances are computed using Equation 4 as, $$\begin{array}{rl}{\mathrm{\Sigma }}_{yy}& =\text{cov}({\hat{y}}_{k+1})=\text{cov}(g({\hat{x}}_{k+1|k})+\upsilon )\\ \text{(13.1)}& & =\sum _{i=1}^{2n}{w}^{(i)}(g(x^{(i)})-{\mu }_y)(g(x^{(i)})-{\mu }_y{)}^T+Q\text{(13.2)}& {\mathrm{\Sigma }}_{xy}& =\text{cov}({\hat{x}}_{k+1|k},{\hat{y}}_{k+1})=\sum _{i=1}^{2n}{w}^{(i)}(f(x^{(i)})-{\mu }_{k+1|k})(g(x^{(i)})-{\mu }_y{)}^T\end{array}$$

Now we can apply equations 9,10 and 11 to obtain the updates mean ${\mu }_{k+1|k+1}$ and covariance ${\mathrm{\Sigma }}_{k+1|k+1}$.

As compared to the Extended Kalman Filter, the UKF is a better approximation for nonlinear systems. Of course, if the system is linear, both EKF and UKF are equivalent to a Kalman Filter. In practice, we typically use the UKF with some tuning parameters in the Unscented Transform. An important point to remember about both the UKF and EKF is that even if they can handle nonlinear systems, they still approximate the filtering distribution $P(x_k|y_1,...,y_k)$ as a Gaussian.

Still wondering why it is called Unscented Kalman Filter?
The guys who invented UKF thought that EKF stinks because it was a very poor idea to linearize a non linear function around a single point i.e. mean. They called it Unscented on purpose so that they can tell the world that EKF stinks!!