Sigmapoint (Unscented) Kalman Filter

A type of Kalman Filter that gets rid of the idea of linearizing altogether and instead use Sigmapoint (Unscented) Transformation.

Setup

Remember, we are trying to solve NLNG Problem Statement

It can be characterized as:

$$\text{motion model:}{\mathbf{x}}_k=\mathbf{f}({\mathbf{x}}_{k-1},{\mathbf{v}}_k,{\mathbf{w}}_k),k=1\dots K$$ $$\text{observation model:}{\mathbf{y}}_k=\mathbf{g}({\mathbf{x}}_k,{\mathbf{n}}_k),k=0\dots K$$

Where $\mathbf{f}(\cdot )$ is a non-linear motion model and $\mathbf{g}(\cdot )$ is a non-linear observation model.

Where $k$ is a index in discrete time.

• ${\mathbf{x}}_k\in {\mathbb{R}}^N$ is the state of the system
• ${\mathbf{x}}_0\in {\mathbb{R}}^N\sim \mathcal{N}({\hat{\mathbf{x}}}_0,{\hat{\mathbf{P}}}_0)$ is the initial state of the system
• ${\mathbf{v}}_k\in {\mathbb{R}}^N$ input to the system. might have a mapping to ${\mathbf{v}}_k=\mathbf{B}{\mathbf{u}}_k$
• ${\mathbf{w}}_k\in {\mathbb{R}}^N\sim \mathcal{N}({\hat{\mathbf{x}}}_0,{\mathbf{Q}}_k)$ process noise
• ${\mathbf{y}}_k\in {\mathbb{R}}^N$ measurement
• ${\mathbf{n}}_k\in {\mathbb{R}}^N\sim \mathcal{N}({\hat{\mathbf{x}}}_0,{\mathbf{R}}_k)$ measurement noise

Predict

We stack the previously calculated posterior and our motion noise uncertainty on top of each other.

$${\mathbf{\mu }}_z=\left[\begin{array}{c}{\hat{\mathbf{x}}}_{k-1}\\ 0\end{array}\right]{\mathbf{\Sigma }}_{zz}=\left[\begin{array}{cc}{\hat{\mathbf{P}}}_{k-1}& 0\\ 0& {\mathbf{Q}}_k\end{array}\right]$$

Let $L=dim({\mathbf{\mu }}_z)$

We then retrieve the sigmapoints

$$\mathbf{L}{\mathbf{L}}^T={\mathbf{\Sigma }}_{zz},\text{Cholesky decomposition}$$ $${\mathbf{z}}_0={\mathbf{\mu }}_z$$ $${\mathbf{z}}_i={\mathbf{\mu }}_z+\sqrt{L+\kappa }{\text{col}}_i\mathbf{L}i=0\dots L$$ $${\mathbf{z}}_i={\mathbf{\mu }}_z-\sqrt{L+\kappa }{\text{col}}_i\mathbf{L}$$ $${\alpha }_i=\{\begin{array}{ll}\frac{\kappa }{L+\kappa },& i=0\\ \frac{1}{2}\frac{1}{L+\kappa }& otherwise\end{array}$$

Unstack each sigmapoint back to state and motion noise

$${\mathbf{z}}_i=\left[\begin{array}{c}{\hat{\mathbf{x}}}_{k-1,i}\\ {\mathbf{w}}_{k,i}\end{array}\right]$$

And then pass each point through the nonlinear motion model

$${\check{\mathbf{x}}}_{k,i}=\mathbf{f}({\hat{\mathbf{x}}}_{k-1,i},{\mathbf{v}}_k,{\mathbf{w}}_{k,i})i=0\dots 2L$$

Recombine the transformation to the predicted prior

$${\check{\mathbf{x}}}_k=\sum _{i=0}^{2L}{\alpha }_i{\check{\mathbf{x}}}_{k,i}$$ $${\check{\mathbf{P}}}_k=\sum _{i=0}^{2L}{\alpha }_i({\check{\mathbf{x}}}_{k,i}-{\check{\mathbf{x}}}_k)({\check{\mathbf{x}}}_{k,i}-{\check{\mathbf{x}}}_k{)}^T$$

We now have our predicted priors!

Update

Recall from Bayes Filter and Joint Gaussian PDFs and Generalized Gaussian Filter

$$p({\mathbf{x}}_k|{\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k})=\mathcal{N}(\underset{{\hat{\mathbf{x}}}_k}{\underbrace{{\mu }_{x,k}+{\mathbf{\Sigma }}_{xy}{\mathbf{\Sigma }}_{yy,k}^{-1}({\mathbf{y}}_k-{\mathbf{\mu }}_{y,k})}},\underset{{\hat{\mathbf{P}}}_k}{\underbrace{{\mathbf{\Sigma }}_{xx,k}-{\mathbf{\Sigma }}_{xy,k}{\mathbf{\Sigma }}_{yy,k}^{-1}{\mathbf{\Sigma }}_{yx,k}}})$$

As a result, we can write the generalized Gaussian correction-step equations as

$${\mathbf{K}}_k={\mathbf{\Sigma }}_{xy,k}{\mathbf{\Sigma }}_{yy,k}^{-1}$$ $${\hat{\mathbf{P}}}_k={\check{\mathbf{P}}}_k-{\mathbf{K}}_k{\mathbf{\Sigma }}_{xy,k}^T$$ $${\hat{\mathbf{x}}}_k={\check{\mathbf{x}}}_k+{\mathbf{K}}_k({\mathbf{y}}_k-{\mathbf{\mu }}_{y,k})$$

We can use Sigmapoint (Unscented) Transformation to get values of ${\mathbf{\mu }}_{y,k},{\mathbf{\Sigma }}_{yy,k},\text{and}{\mathbf{\Sigma }}_{xy,k}$.

First, we use Sigmapoint (Unscented) Transformation to handle the non-linearity in the observation model.

We stack the previously calculated posterior and our motion noise uncertainty on top of each other.

$${\mathbf{\mu }}_z=\left[\begin{array}{c}{\check{\mathbf{x}}}_k\\ 0\end{array}\right]{\mathbf{\Sigma }}_{zz}=\left[\begin{array}{cc}{\check{\mathbf{P}}}_k& 0\\ 0& {\mathbf{R}}_k\end{array}\right]$$

Let $L=dim({\mathbf{\mu }}_z)$

We then retrieve the sigmapoints

$$\mathbf{L}{\mathbf{L}}^T={\mathbf{\Sigma }}_{zz},\text{Cholesky decomposition}$$ $${\mathbf{z}}_0={\mathbf{\mu }}_z$$ $${\mathbf{z}}_i={\mathbf{\mu }}_z+\sqrt{L+\kappa }{\text{col}}_i\mathbf{L}i=0\dots L$$ $${\mathbf{z}}_i={\mathbf{\mu }}_z-\sqrt{L+\kappa }{\text{col}}_i\mathbf{L}$$ $${\alpha }_i=\{\begin{array}{ll}\frac{\kappa }{L+\kappa },& i=0\\ \frac{1}{2}\frac{1}{L+\kappa }& otherwise\end{array}$$

Unstack each sigmapoint back to state and motion noise

$${\mathbf{z}}_i=\left[\begin{array}{c}{\check{\mathbf{x}}}_{k-1,i}\\ {\mathbf{n}}_{k,i}\end{array}\right]$$

And then pass each point through the nonlinear motion model

$${\check{\mathbf{y}}}_{k,i}=\mathbf{g}({\check{\mathbf{x}}}_{k-1,i},{\mathbf{n}}_{k,i})i=0\dots 2L$$

Recombine the transformation to the predicted prior (we are computing what we need based on the Generalized Gaussian Filter)

$${\mu }_{y,k}=\sum _{i=0}^{2L}{\alpha }_i{\check{\mathbf{y}}}_{k,i}$$ $${\mathbf{\Sigma }}_{yy,k}=\sum _{i=0}^{2L}{\alpha }_i({\check{\mathbf{y}}}_{k,i}-{\mathbf{\mu }}_{y,k})({\check{\mathbf{y}}}_{k,i}-{\mathbf{\mu }}_{y,k}{)}^T$$ $${\mathbf{\Sigma }}_{xy,k}=\sum _{i=0}^{2L}{\alpha }_i({\check{\mathbf{x}}}_{k,i}-{\mathbf{x}}_k)({\check{\mathbf{y}}}_{k,i}-{\mathbf{\mu }}_{y,k}{)}^T$$

Plug into

$${\mathbf{K}}_k={\mathbf{\Sigma }}_{xy,k}{\mathbf{\Sigma }}_{yy,k}^{-1}$$ $${\hat{\mathbf{P}}}_k={\check{\mathbf{P}}}_k-{\mathbf{K}}_k{\mathbf{\Sigma }}_{xy,k}^T$$ $${\hat{\mathbf{x}}}_k={\check{\mathbf{x}}}_k+{\mathbf{K}}_k({\mathbf{y}}_k-{\mathbf{\mu }}_{y,k})$$

and we have our updated posterior!