# 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