# Kalman Filter 

##### Definition

The Kalman Filter is a linear quadratic estimator used to estimate the state of a system by combining sensor measurements and a (physical) process model of the system. It works optimal if measurements and process variations have white gaussian noise.

## Working Principle

The Kalman Filter averages a prediction of a system's state with a new measurement using a weighted average. The purpose of the weights is that estimated values with smaller uncertainty are "trusted" more. The result of the weighted average is a new state estimate that lies between the predicted and measured state, and has a smaller estimated uncertainty than either alone.

The Kalman filter works in two steps: predict and update.

## Defining the Model

• system's process model
• control inputs to that process
• multiple sequential measurements (e.g. from sensors)

### State Space

The dynamic model for the physical process is

$$\begin{array}{ll} {\boldsymbol x}_{n+1} & = {\boldsymbol{G}}_n {\boldsymbol x}_{n} + {\boldsymbol{B}} {\boldsymbol u}_n + {\boldsymbol w}_n \\ {\boldsymbol y}_{n} & = {\boldsymbol{H}}_{n} {\boldsymbol x}_{n} + {\boldsymbol v}_{n} \end{array}$$

with the $k$ states ${\boldsymbol x}$, transition matrix ${\boldsymbol{G}}$, gaussian process noise ${\boldsymbol w}_n$, input ${\boldsymbol u}$, $l$ measurements ${\boldsymbol y}$, measurement model ${\boldsymbol{H}}$, gaussian measurement noise ${\boldsymbol v}_n$, time point $n$.

The measurement matrix ${\boldsymbol{H}}$ defines how the observations correspond to the state. If the state variables can be directly observed then ${\boldsymbol{H}} = {\boldsymbol{1}}$.

If there are no known inputs to the process, then ${\boldsymbol{B}} {\boldsymbol u}_n = 0$ and this term can be removed.

### Noise

The overall uncertainty of the estimation is expressed with the covariance matrix ${\boldsymbol{P}}_n$. This matrix is influenced by the process noise ${\boldsymbol w}_n$ and the measurement noise ${\boldsymbol v}_n$. During prediction, the process noise increases the uncertainty, whereas combining it with the measurement during the update phase decreases the uncertainty.

$$\begin{array}{ll} {\boldsymbol w}_n & \sim \mathcal{N}({\boldsymbol{0}}, {\boldsymbol{Q}}_n) \\ {\boldsymbol v}_n & \sim \mathcal{N}({\boldsymbol{0}}, {\boldsymbol{R}}_n) \end{array}$$

If values of ${\boldsymbol{Q}}$ are larger than values of ${\boldsymbol{R}}$, the filter trusts more the process, less the measurements.

## Calculations

### 1. Step: Prediction

1.1 calculate the new (a priori) state estimate based on the old state and the dynamic model (e.g. physical laws)

$$\hat {{\boldsymbol x}}_{n|n-1} = {\boldsymbol{G}}_n \hat{{\boldsymbol x}}_{n-1|n-1}$$

1.2 calculate a new process covariance (how certain is the model?)

$${\boldsymbol{P}}_{{\boldsymbol x}_{n|n-1}} = {\boldsymbol{G}}_n {\boldsymbol{P}}_{{\boldsymbol x}_{n-1|n-1}} {\boldsymbol{G}}_n^\top + {\boldsymbol{Q}}_n$$

### 2. Step: Update

2.1 calculate intermediate values (optional):

• Innovation: $\Delta {\boldsymbol y}_n = {\boldsymbol y}_n - \hat{{\boldsymbol y}}_{n|n-1} ={\boldsymbol y}_n - {\boldsymbol{H}}_{n} \hat{{\boldsymbol x}}_{n|n-1}$
which are the real measurements minus predicted measurements

• Innovation Covariance: ${\boldsymbol{S}} = {\boldsymbol{H}}_{n} {\boldsymbol{P}}_{{\boldsymbol x}_{n|n-1}} {\boldsymbol{H}}_{n}^\top + {\boldsymbol{R}}_n$

2.2 calculate optimal Kalman-gain:

$${\boldsymbol{K}}_n = \frac{{\boldsymbol{P}}_{{\boldsymbol x}_{n|n-1}} {\boldsymbol{H}}_{n}^\top}{{\boldsymbol{H}}_{n} {\boldsymbol{P}}_{{\boldsymbol x}_{n|n-1}} {\boldsymbol{H}}_{n}^\top + {\boldsymbol{R}}_n} = {\boldsymbol{P}}_{{\boldsymbol x}_{n|n-1}} {\boldsymbol{H}}_{n}^\top {{\boldsymbol{S}}}^{-1}$$

2.3 calculate the updated (a posteriori) state estimate using $l$ measurements:

$$\hat{{\boldsymbol x}}_{n|n} = \hat{{\boldsymbol x}}_{n|n-1} + {\boldsymbol{K}}_n \Delta {\boldsymbol y}_n$$ which is the estimation of ${\boldsymbol x}_n$ based on $\Delta y_n$ with $K_{ij} \in [0.0; 1.0]$ where 0.0 means the filter fully trusts the prediction and 1.0 means the filter fully trusts the measurement.

2.4 update process covariance:

$${\boldsymbol{P}}_{{\boldsymbol x}_{n|n}} = {\boldsymbol{P}}_{{\boldsymbol x}_{n|n-1}} + {\boldsymbol{K}}_n {\boldsymbol{H}}_{n} {\boldsymbol{P}}_{{\boldsymbol x}_{n|n-1}}$$

## Extended Kalman Filter

The Extended Kalman Filter (EKF) uses non-linear dynamic models.

$$\begin{array}{ll} {\boldsymbol x}_{n} & = g({\boldsymbol x}_{n-1}, {\boldsymbol u}_n) + {\boldsymbol w}_n \\[0.5em] {\boldsymbol y}_{n} & = h({\boldsymbol x}_{n-1}) + {\boldsymbol v}_{n} \end{array}$$ where $g()$ and $h()$ are non-linear functions. For covariance the Jacobi-Matrix of the model is used:

$${\boldsymbol{G}} = \frac{\partial g}{\partial x} \Big\vert_{\hat{{\boldsymbol x}}_{n-1}, {\boldsymbol u}_n} \qquad {\boldsymbol{H}} = \frac{\partial h}{\partial x} \Big\vert_{\hat{{\boldsymbol x}}_{n}}$$

## Sensor Fusion

If $i$ sensors measure the same state $x_j$, this can be expressed in the measurement matrix ${\boldsymbol{H}}$. The column $j$ will have $i$ rows with entries.

##### Example

Kalman Filter for Gyroscope and Accelerometer:

state ${\boldsymbol x}$ are the orientation angles roll and pitch and the bias angle

-->