Generalized Gaussian Filter

Bayes Filter Provides us a fundamental framework for designing frameworks. However, it is very high-level, and its generalized to all PDF. We don’t need something so generalized in Engineering ;) so we can look specifically at a subset of filters that assume Gaussian PDFs up front.

Recall Bayes Filter is

$$\underset{\text{posterior belief}}{\underbrace{p({\mathbf{x}}_k|{\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k})}}=\eta \underset{\begin{array}{c}\text{observation}\\ \text{correction}\\ \text{using}\mathbf{g}(\cdot )\end{array}}{\underbrace{p({\mathbf{y}}_k|{\mathbf{x}}_k)}}\int \underset{\begin{array}{c}\text{motion prediction}\\ \text{using }\mathbf{f}(\cdot )\end{array}}{\underbrace{p({\mathbf{x}}_k|{\mathbf{x}}_{k-1},{\mathbf{v}}_k)}}\underset{\text{prior belief}}{\underbrace{p({\mathbf{x}}_{k-1}|{\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k-1},{\mathbf{y}}_{0:k-1})}}d{\mathbf{x}}_{k-1}$$

Prediction

In general, we begin by assuming a Gaussian prior at time $k-1$

$$p({\mathbf{x}}_{k-1}|{\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k-1},{\mathbf{y}}_{0:k-1})=\mathcal{N}({\hat{\mathbf{x}}}_{k-1},{\hat{\mathbf{P}}}_{k-1})$$

We then assume that passing this though a non-linear motion model $\mathbf{f}(\cdot )$ is gonna give us another Gaussian

$$p({\mathbf{x}}_k|{\check{\mathbf{x}}}_0,{\mathbf{v}}_k,{\mathbf{y}}_{0:k-1})=\mathcal{N}({\check{\mathbf{x}}}_k,{\check{\mathbf{P}}}_k)$$

This is quite an assumption that we do up front

This is our prediction prior

Update

Let’s assume that our posterior is going to be Gaussian in nature.

From Joint Gaussian PDFs we can write a Joint Gaussian with the state and the measurement.

$$p({\mathbf{x}}_k,{\mathbf{y}}_k|{\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k-1})=\mathcal{N}\left(\left[\begin{array}{c}{\mathbf{\mu }}_{x,k}\\ {\mathbf{\mu }}_{y,k}\end{array}\right],\left[\begin{array}{cc}{\mathbf{\Sigma }}_{xx,k}& {\mathbf{\Sigma }}_{xy,k}\\ {\mathbf{\Sigma }}_{yx,k}& {\mathbf{\Sigma }}_{yy,k}\end{array}\right]\right)$$

From what we saw in Joint Gaussian PDFs we can split up the joint probability into two factors, both being Gaussian with means and covariances comprised of the factorized quadratic part of the joint distribution

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

Lining this up with our prediction prior

$$p({\mathbf{x}}_k|{\check{\mathbf{x}}}_0,{\mathbf{v}}_k,{\mathbf{y}}_{0:k-1})=\int p({\mathbf{x}}_k,{\mathbf{y}}_k|{\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k-1})d{\mathbf{y}}_k=\mathcal{N}({\mathbf{\mu }}_{x,k},{\mathbf{\Sigma }}_{xx,k})=\mathcal{N}({\check{\mathbf{x}}}_k,{\check{\mathbf{P}}}_k)$$

Here we took the marginal of the joint gaussian distribution. Which obviously just gives us the distribution of the variable that wasn't integrated out! This thus gives us a direct connection between our prediction prior and our join Gaussian model!!!

$${\mathbf{\mu }}_{x,k}={\check{\mathbf{x}}}_k,{\mathbf{\Sigma }}_{xx,k}={\check{\mathbf{P}}}_k$$

Injecting our prediction prior, we see that we end up with the generalized Gaussian correction step!

$${\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})$$