# Vanilla Kalman Filter

## Graphical model

![Hidden states are denoted as "x" and observables as "z"](https://3015635107-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-MIjnN3E1djiDcT0zdOC%2F-MIjxizAIIGIk9BW3cZZ%2F-MIjy8f2cWyEFr-TsWuF%2Fkalman_graph.svg?alt=media\&token=5df8618f-f49d-4665-9918-83001d5f528b)

## Kindergarten physics

$$
{\bf r}(t+\delta t) = {\bf r}(t) + \dot {\bf r}(t) \delta t + \frac12 \ddot {\bf r}(t) \delta t^2 + {\rm h.o.t}
$$

If assume $$\langle \ddot {\bf r} \rangle = 0$$, then the state $$x$$ is characterized by $$({\bf r}, \dot {\bf r})$$, i.e.

$$
x\_{i}(t+\delta t) = x\_i (t) + v\_i (t) \delta t +\frac12 \varepsilon\_i(t) \delta t^2\ v\_{i}(t+\delta t) = v\_i(t) + \varepsilon\_i (t) \delta t \qquad\qquad\qquad
$$

Time evolution

$$
x\_a(t+\delta t) = F\_{ab} x\_b(t) + \eta\_a
$$

$$
F\_{ab} = \pmatrix{ 1 & \delta t \ 0 & 1 }\quad \eta\_a \sim {\cal N}(0, {Q})
$$

Observation or measurement

$$
z\_a(t) = H\_{ab} x\_b(t) + \xi\_a\quad \ H\_{ab} = \pmatrix{ 1 & 0 \ } \quad \xi\_a\sim {\cal N}(0, {R})
$$

## Inference

* Given observations $$z\_{\<t}$$ up to $$t-1$$, what is the distribution of state $$x\_t$$?

$$
p(x\_t|z\_{\<t}) = \int {\rm d}x\_{t-1}; p(x\_t | x\_{t-1}) p (x\_{t-1} | z\_{\le t-1})
$$

* If there is one more observation $$z\_t$$, what is the distribution of state $$x\_t$$?

$$
p(x\_t|z\_{\le t}) = p(x\_t | z\_t, z\_{\<t}) \propto p(z\_t | x\_t) p(x\_t | z\_{\<t})
$$

Expand the recursion, we have Feynman-Kac formula or path integral

$$
p(x\_t | z\_{< t}) \propto \int \left \[ \prod\_{\tau = 1}^{t-1} {\rm d}x\_\tau \right], \prod *{\tau=1}^{t}p(z*\tau | x\_\tau) p(x\_\tau | x\_{\tau - 1}) p(z\_0 | x\_0) p(x\_0)
$$

If we assume the priors, transition probabilities and emission probabilities are all normal, then the posterior of state $$x\_t$$ is also normal.

Thus, we assume

$$
p(x\_t | z\_{< t}) = \mathcal N (\hat x\_{t|t-1}, {P}*{t|t-1})\ p(x\_t | z*{\le t}) = \mathcal N (\hat x\_{t|t}, {P}\_{t|t})\qquad
$$

### State means

Use the recursion again,

$$
p(x\_t | z\_{\le t}) \propto p(z\_t | x\_t) p(x\_t | z\_{\<t})
$$

$$
\mathcal N(x\_t; \hat x\_{t|t}, P\_{t|t}) \propto \mathcal N(z\_t; Hx\_t, R) \mathcal N(x\_t; \hat x\_{t|t-1}, P\_{t|t-1})
$$

Maximize $$x\_t$$ \_\_ on both sides, the LHS \_\_ $$x\_t = \hat x \_{t|t}$$, the RHS

$$
{\rm rhs}=(z\_t - Hx\_t|R^{-1}|z\_t - Hx\_t) + (x\_t - \hat x\_{t|t-1}|P\_{t|t-1}^{-1}|x\_t - \hat x\_{t|t-1})
$$

$$
0=\partial\_{x\_t^\top}{\rm rhs}= - H^\top R^{-1}(z\_t - Hx\_t) + P^{-1}*{t|t-1} (x\_t - \hat x*{t|t-1})
$$

We must have

$$
\begin{align} (\hat x\_{t|t} - \hat x\_{t|t-1}) &= P\_{t|t-1} H^\top R^{-1} (z\_t - H \hat x\_{t|t})\ &= P\_{t|t-1} H^\top R^{-1} (z\_t - H \hat x\_{t|t-1} - H(\hat x\_{t|t} - \hat x\_{t|t-1}) ) \end{align}
$$

Therefore,

$$
\hat x\_{t|t} - \hat x\_{t|t-1} = (I + P\_{t|t-1} H^\top R^{-1} H)^{-1} P\_{t|t-1} H^\top R^{-1} (z\_t - H \hat x\_{t|t}) \equiv K (z\_t - H \hat x\_{t|t})
$$

where $$K = (I + P\_{t|t-1} H^\top R^{-1} H)^{-1} P\_{t|t-1} H^\top R^{-1}$$ is the *Kalman gain*.

### State covariances

The covariances

$$
(P\_{t|t-1})*{ab} \equiv \langle (x\_t - \hat x*{t|t-1})*a (x\_t - \hat x*{t|t-1})\_b \rangle
$$

$$
(P\_{t|t})*{ab} \equiv \langle (x\_t - \hat x*{t|t})*a (x\_t - \hat x*{t|t})\_b \rangle
$$

where

$$
\begin{align} (P\_{t|t})*{ab} &= \langle \[x\_t - (\hat x*{t|t} - \hat x\_{t|t-1}) - \hat x\_{t|t-1}]*a \[\cdots]*b \rangle \ &= \langle \[x\_t - \hat x*{t|t-1} + (\hat x*{t|t} - \hat x\_{t|t-1})]*a \[\cdots]*b \rangle \ &= \langle \[x\_t - \hat x*{t|t-1} - K (z\_t - H\hat x*{t|t-1})]*a \[\cdots]*b \rangle \ &= \langle \[x\_t - \hat x*{t|t-1} - K (H(x\_t-\hat x*{t|t-1}) + \xi)]*a \[\cdots]*b \rangle \ &= \langle \[(I-KH)*{ac}(x\_t - \hat x*{t|t-1})*c - K*{ac}\xi\_c]\[(I-KH)*{bd}(x*{t} - \hat x\_{t|t-1})*d - K*{bd}\xi\_d] \rangle\ &= (I - KH) P\_{t|t-1} (I - KH)^\top + KRK^\top \end{align}
$$

Given the prior covariance, minimize the posterior MSE $$\min\_K (P\_{t|t})\_{aa}$$

$$
\begin{align} \frac{\partial, {\rm tr} P\_{t|t}}{\partial K} &= (-HP\_{t|t-1}(I-KH)^\top + RK^\top)^\top \ &= KR - (I-KH)P\_{t|t-1}^\top H^\top \ &= K (R + HP\_{t|t-1}^\top H^\top) - P\_{t|t-1}^\top H^\top \end{align}
$$

Therefore,

$$
K^\* = P\_{t|t-1}^\top H^\top (HP\_{t|t-1}^\top H^\top + R)^{-1}
$$

But previously we got $$K = (I + P\_{t|t-1} H^\top R^{-1} H)^{-1} P\_{t|t-1} H^\top R^{-1}$$.

Are they consistent? To show that

$$
\begin{align} K^{-1} &= R (P\_{t|t-1}H^\top)^{-1}(I + P\_{t|t-1} H^\top R^{-1} H) \ &= R(P\_{t|t-1}H^\top)^{-1} + H \ &= (R + HP\_{t|t-1}H^\top)(P\_{t|t-1}H^\top)^{-1} \end{align}
$$

Thus, $$K = (P\_{t|t-1}H^\top) (R + HP\_{t|t-1}H^\top)^{-1} = K^\*$$

Given the posterior, the prior covariance for next step

$$
\begin{align} (P\_{t|t-1})*{ab} &\equiv \langle (x\_t - \hat x*{t|t-1})*a (x\_t - \hat x*{t|t-1})*b \rangle \ &= \langle (x\_t - F\hat x*{t-1|t-1})*a (x\_t - F\hat x*{t-1|t-1})*b \rangle \ &= \langle (x\_t - F x*{t-1|t-1} + F(x\_{t-1|t-1} - \hat x\_{t-1|t-1}) )*a (x\_t - F x*{t-1|t-1} + F(x\_{t-1|t-1} - \hat x\_{t-1|t-1}))\_b \rangle \end{align}
$$

Thus, the forecasted covariance the composed of measurement noise and evolution noise

$$
P\_{t|t-1} = Q + F P\_{t-1|t-1} F^\top
$$

### Algorithm

* Forecast state mean using posterior as prior

$$
\hat x\_{t|t-1} = F\hat x\_{t-1 | t-1}\ P\_{t|t-1} = Q + F P\_{t-1|t-1} F^\top
$$

* Compute Kalman gain $$K=(P\_{t|t-1}H^\top)(R + HP\_{t|t-1}H^\top)^{-1}$$
* Make a measurement $$z\_t$$and a prediction using forecast state \_\_ $$H\hat x\_{t|t-1}$$\_\_
* Compute posterior mean using new observation $$\hat x\_{t|t} = \hat x\_{t|t-1} + K(z\_t - H\hat x\_{t|t-1})$$
* Compute posterior covariance $$P\_{t|t} = (I -KH)P\_{t|t-1}(I-KH)^\top + K R K^\top$$
