> For the complete documentation index, see [llms.txt](https://calvinfeng.gitbook.io/probabilistic-robotics/llms.txt). Markdown versions of documentation pages are available by appending `.md` to page URLs; this page is available as [Markdown](https://calvinfeng.gitbook.io/probabilistic-robotics/basics/gaussian-filters/01-kalman-filter.md).

# Kalman Filter

## Linear Gaussian Systems

The Kalman filter implements belief computational for continuous state. It is not applicable to discrete or hybrid state spaces. The filter represents belief by moments parameterization. At time $$t$$, the belief is represented by the mean $$\mu\_{t}$$ and the covariance $$\Sigma\_{t}$$. Posteriors are Gaussian if the following three properties hold in addition to Markov assumption. 

### 1. Linear Transition

The state transition probability $$p(x\_{t} \mid u\_{t}, x\_{t-1})$$ must be a linear function in its arguments with added Gaussian noises.

$$
\tag{1} x\_{t} = A\_{t}x\_{t-1} + B\_{t}u\_{t} + \epsilon\_{t}
$$

The state and control are vectors. In our notation, they are vertical vectors.

$$
x\_{t} = \begin{vmatrix}
x\_{t}\[0] \ x\_{t}\[1] \ x\_{t}\[2] \ ... \ x\_{t}\[n-1]\
\end{vmatrix}
\quad
u\_{t} = \begin{vmatrix}
u\_{t}\[0] \ u\_{t}\[1] \ u\_{t}\[2] \ ... \ u\_{t}\[m-1]\
\end{vmatrix}
$$

Here $$A\_{t}$$ and $$B\_{t}$$ are matrices. $$A\_{t}$$ is of size n by n and $$B\_{t}$$ is of size n by m. The dimension of state vector is n and the dimension of control vector is m. The random variable $$\epsilon\_{t}$$ has the same dimension as the state vector. Its mean is zero and its covariance will be denoted as $$R\_{t}$$. It is there to modifies the uncertainty introduced by the state transition. The mean of the posterior state is given by equation 1 and covariance by $$R\_{t}$$.

$$
p(x\_{t} \mid u\_{t}, x\_{t-1}) = det(2\pi R\_{t})^{-0.5} ; \exp\left\[
\frac{-1}{2} (x\_{t} - A\_{t}x\_{t-1} - B\_{t}u\_{t})^{T} R\_{t}^{-1} (x\_{t} - A\_{t}x\_{t-1} - B\_{t}u\_{t})
\right]
$$

### 2. Linear Measurement

The measurement probability $$p(z\_{t} \mid x\_{t})$$ must also be linear with added Gaussian noise.

$$
\tag{2} z\_{t} = C\_{t}x\_{t} + \delta\_{t}
$$

Here $$C\_{t}$$is a matrix of size k by n where k is the dimension of the measurement vector. The $$\delta$$ describes the measurement noise. The distribution of noise is a multivariate Gaussian with zero mean and covariance of $$Q\_{t}$$. 

$$
p(z\_{t}\mid x\_{t}) = det(2\pi Q\_{t})^{-0.5} \exp
\left\[
\frac{-1}{2}(z\_{t} - C\_{t}x\_{t})^{T}Q\_{t}^{-1}
(z\_{t} - C\_{t}x\_{t})
\right]
$$

### 3. Normal Belief

The initial belief $$bel(x\_{0})$$ must be normally distributed with mean $$\mu\_{0}$$ and covariance $$\Sigma\_{0}$$.

$$
bel(x\_{0})= p(x\_{0}) = det(2\pi\Sigma\_{0})^{-0.5} \exp\left\[
\frac{-1}{2}(x\_{0} - \mu\_{0})^{T} \Sigma\_{0}^{T} (x\_{0} - \mu\_{0})
\right]
$$

## Kalman Filter Algorithm

Given arguments $$\mu\_{t-1}$$, $$\Sigma\_{t-1}$$, $$u\_{t}$$, and $$z\_{t}$$, we have the following update rules.

$$
\tag{3a} \overline{\mu\_{t}} = A\_{t}\mu\_{t-1} + B\_{t}u\_{t} \\;\\
\overline{\Sigma\_{t}} = A\_{t}\Sigma\_{t-1}A\_{t-1}^{T} + R\_{t}
$$

The predicted belief $$\overline{\mu\_{t}}$$ and $$\overline{\Sigma\_{t}}$$ are calculated to represent the belief $$\overline{bel}(x\_{t})$$, one time step later, but before incorporating the measurement $$z\_{t}$$.

$$
\tag{3b} K\_{t} = \overline{\Sigma}*{t} C*{t}^{T} (C\_{t}\overline{\Sigma}C\_{t}^{T} + Q\_{T})^{-1}
$$

Before we perform the measurement update, we need to compute Kalman gain from equation 3b, which specifies the degree to which the measurement is incorporated into the new state estimate. Then we use the gain to get the new state.

$$
\tag{3c} \mu\_{t} = \overline{\mu\_{t}} + K\_{t}(z\_{t} - C\_{t}\overline{\mu\_{t}}) \\;\\
\Sigma\_{t} = (I - K\_{t}C\_{t})\overline{\Sigma\_{t}}
$$

The key concept here is innovation, which is the difference between the actual measurement and expected measurement, denoted by $$z\_{t} - C\_{t}\overline{\mu\_{t}}$$ .

### Code Example

For simplicity sake, I will omit the time dependence for transformation matrices. Let's define three matrices `A`, `B`, and `C` using `numpy` . Note that `A` is the state transition model or function, `B` is the control input model, and `C` is the observation model. 

```python
A = np.array([[1.0, 1.0], [0.0, 1.0]])
B = np.array([[1.0, 0.0], [0.0, 1.0]])
C = np.array([[1.0, 0.0]])
```

If we were to express them in matrix form, they would look like the following.

$$
A = \begin{vmatrix}
1.0 & 1.0 \ 0.0 & 1.0
\end{vmatrix}
\\;\\
B = \begin{vmatrix}
1.0 & 0.0 \ 0.0 & 1.0
\end{vmatrix}
\\;\\
C = \begin{vmatrix}
1.0 & 0.0
\end{vmatrix}
$$

Suppose our robot is at coordinate $$(0.0, 0.0)$$ initially and we don't apply any external control to it. Let's denote `x` to be $$\mu$$, `x_cov` to be $$\Sigma$$, `u` to be $$u$$, and `u_cov` to be $$R$$.

```python
x = np.array([[0.0], [0.0]])
x_cov = np.array([[1000.0, 0.0], [0.0, 1000.0]])
u = np.array([[0.0], [0.0]])
u_cov = np.array([[0.0, 0.0], [0.0, 0.0]])
```

$$
\mu = \begin{vmatrix}
0 \ 0
\end{vmatrix}
;
\Sigma = \begin{vmatrix}
1000.0 & 0.0 \ 0.0 & 1000.0
\end{vmatrix}
\\;\\
u = \begin{vmatrix}
0.0 \ 0.0
\end{vmatrix};
R = \begin{vmatrix}
0.0 & 0.0 \ 0.0 & 0.0
\end{vmatrix}
$$

Let `Z` to be $$(z\_{0}, z\_{1}, z\_{2}, ...)$$and `z_cov` to be $$Q$$.

```python
Z = [np.array([[1]]), np.array([[2]]), np.array([[3]])
z_cov = np.array([[1]])
```

$$
z\_{0} = \begin{vmatrix} 1 \end{vmatrix} ; z\_{1} = \begin{vmatrix} 2 \end{vmatrix} etc...\\;\\
Q = \begin{vmatrix} 0 \end{vmatrix}
$$

Now we can put everything together and construct a Kalman filter algorithm.

```python
def predict_new_belief(x, x_cov, u, u_cov):
    x = dot(A, x) + dot(B, u)
    x_cov = dot(dot(A, x_cov), A.T) + u_cov
    return x, x_cov


def incorporate_measurement(x, x_cov, z, z_cov):
    S = dot(dot(C, x_cov), C.T) + z_cov
    K = dot(dot(x_cov, C.T), linalg.inv(S))

    x = x + dot(K, z - C.dot(x))
    x_cov = dot(identity(2) - dot(K, C), x_cov)
    return x, x_cov


for i in range(len(Z)):
    x, x_cov = predict_new_belief(x, x_cov, u, u_cov)
    x, x_cov = incorporate_measurement(x, x_cov, Z[i], z_cov)
```