Summary

We have derived all five Kalman Filter equations in matrix notation. Let us put them all together on a single page.

The Kalman Filter operates in a “predict-correct” loop, as shown in the diagram below.

Get the book

Once initialized, the Kalman Filter predicts the system state at the next step. It also provides the uncertainty of the prediction.

Once the measurement is received, the Kalman Filter updates (or corrects) the prediction and the uncertainty of the current state. As well the Kalman Filter predicts the following states, and so on.

The following diagram provides a complete picture of the Kalman Filter operation.

The following table describes all Kalman Filter Equations.

	Equation	Equation Name	Alternative names
Predict	$\boldsymbol{\hat{x}}_{n+1,n} = \boldsymbol{F\hat{x}}_{n,n} + \boldsymbol{Gu}_{n}$	State Extrapolation	Predictor Equation Transition Equation Prediction Equation Dynamic Model State Space Model
Predict	$\boldsymbol{P}_{n+1,n} = \boldsymbol{FP}_{n,n}\boldsymbol{F}^{T} + \boldsymbol{Q}$	Covariance Extrapolation	Predictor Covariance Equation
Update (correction)	$\boldsymbol{\hat{x}}_{n,n} = \boldsymbol{\hat{x}}_{n,n-1} + \boldsymbol{K}_{n} (\boldsymbol{z}_{n} - \boldsymbol{H\hat{x}}_{n,n-1} )$	State Update	Filtering Equation
	$\boldsymbol{P}_{n,n} = \left(\boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \boldsymbol{P}_{n,n-1} \left(\boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right)^{T} + \boldsymbol{K}_{n}\boldsymbol{R}_{n}\boldsymbol{K}_{n}^{T}$	Covariance Update	Corrector Equation
	$\boldsymbol{K}_{n} = \boldsymbol{P}_{n,n-1}\boldsymbol{H}^{T}\left(\boldsymbol{HP}_{n,n-1}\boldsymbol{H}^{T} + \boldsymbol{R}_{n} \right)^{-1}$	Kalman Gain	Weight Equation
Auxiliary	$\boldsymbol{z}_{n} = \boldsymbol{Hx}_{n}$	Measurement Equation
	$\boldsymbol{R}_{n} = E\left( \boldsymbol{v}_{n}\boldsymbol{v}_{n}^{T} \right)$	Measurement Covariance	Measurement Error
	$\boldsymbol{Q}_{n} = E\left( \boldsymbol{w}_{n}\boldsymbol{w}_{n}^{T} \right)$	Process Noise Covariance	Process Noise Error
	$\boldsymbol{P}_{n,n} = E\left( \boldsymbol{e}_{n}\boldsymbol{e}_{n}^{T} \right) = E\left( \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n} \right)^{T} \right)$	Estimation Covariance	Estimation Error

The following table summarizes notation (including differences found in the literature) and dimensions.

Term	Name	Alternative term	Dimensions
$\boldsymbol{x}$	State Vector		$n_{x} \times 1$
$\boldsymbol{z}$	Measurements Vector	$\boldsymbol{y}$	$n_{z} \times 1$
$\boldsymbol{F}$	State Transition Matrix	$\boldsymbol{\Phi,A}$	$n_{x} \times n_{x}$
$\boldsymbol{u}$	Input Variable		$n_{u} \times 1$
$\boldsymbol{G}$	Control Matrix	$\boldsymbol{B}$	$n_{x} \times n_{u}$
$\boldsymbol{P}$	Estimate Covariance	$\boldsymbol{\Sigma}$	$n_{x} \times n_{x}$
$\boldsymbol{Q}$	Process Noise Covariance		$n_{x} \times n_{x}$
$\boldsymbol{R}$	Measurement Covariance		$n_{z} \times n_{z}$
$\boldsymbol{w}$	Process Noise Vector	$\boldsymbol{y}$	$n_{x} \times 1$
$\boldsymbol{v}$	Measurement Noise Vector		$n_{z} \times 1$
$\boldsymbol{H}$	Observation Matrix	$\boldsymbol{C}$	$n_{z} \times n_{x}$
$\boldsymbol{K}$	Kalman Gain		$n_{x} \times n_{z}$
$\boldsymbol{n}$	Discrete-Time Index	$\boldsymbol{k}$

Dimensions notation:

$n_{x}$ is a number of states in a state vector
$n_{z}$ is a number of measured states
$n_{u}$ is a number of elements of the input variable

Summary

Cookies