## Introduction to Estimation Filters

### Background

#### Estimation Systems

For many autonomous systems, the knowledge of the system state is a prerequisite for designing any applications. In reality, however, the state is often not directly obtainable. The system state is usually inferred or estimated based on the system outputs measured by certain instruments (such as sensors) and the flow of the state governed by a dynamic or motion model. Some simple techniques, such as least square estimation or batch estimation, are sufficient in solving static or offline estimation problems. For online and real time (sequential) estimation problems, more sophisticated estimation filters are usually applied.

An estimation system is composed of a dynamic or motion model that describes the flow of the state and a measurement model that describes how the measurements are obtained. Mathematically, these two models can be represented by an equation of motion and a measurement equation. For example, the equation of motion and measurement equation for a general nonlinear discrete estimation system can be written as:

$$\begin{array}{l}{x}_{k+1}=f({x}_{k})\\ \text{\hspace{0.17em}}\text{\hspace{0.17em}}\text{\hspace{0.05em}}\text{\hspace{0.05em}}\text{}{y}_{k}=h({x}_{k})\end{array}$$

where *k* is the time step,
*x*_{k
} is the system state at time step *k*,
*f*(*x*_{k})
is the state-dependent equation of motion,
*h*(*x*_{k})
is the state dependent measurement equation, and
*y*_{k } is the
output.

#### Noise Distribution

In most cases, building a perfect model to capture all the dynamic phenomenon is
not possible. For example, including all frictions in the motion model of an
autonomous vehicle is impossible. To compensate for these unmodeled dynamics,
process noise (*w*) is often added to the dynamic model. Moreover,
when measurements are taken, multiple sources of errors, such as calibration errors,
are inevitably included in the measurements. To account for these errors, proper
measurement noise must be added to the measurement model. An estimation system
including these random noises and errors is called a stochastic estimation system,
which can be represented by:

$$\begin{array}{l}{x}_{k+1}=f({x}_{k},{w}_{k})\\ \text{\hspace{0.17em}}\text{\hspace{0.17em}}\text{\hspace{0.05em}}\text{\hspace{0.05em}}\text{}{y}_{k}=h({x}_{k},{v}_{k})\end{array}$$

where *w*_{k
} and *v*_{k
}represent process noise and measurement noise, respectively.

For most engineering applications, the process noise and measurement noise are assumed to follow zero-mean Gaussian or normal distributions, or are at least be approximated by Gaussian distributions. Also, because the exact state is unknown, the state estimate is a random variable, usually assumed to follow Gaussian distributions. Assuming Gaussian distributions for these variables greatly simplifies the design of an estimation filter, and form the basis of the Kalman filter family.

A Gaussian distribution for a random variable (*x*) is
parametrized by a mean value *μ* and a covariance matrix
*P*, which is written as
*x*∼*N*(*μ*,*P*).
Given a Gaussian distribution, the mean, which is also the most likely value of
*x*, is defined by expectation (*E*) as:

$$\mu =E[x]$$

The mean is also called the first moment of *x*
about the origin. The covariance that describes of the uncertainty of
*x* is defined by expectation (*E*) as:

$$P=E\left[\left(x-\mu \right){\left(x-\mu \right)}^{T}\right]$$

The covariance is also called the second moment of
*x* about its mean.

If the dimension of *x* is one, *P* is only a
scalar. In this case, the value of *P* is usually denoted by
*σ*^{2} and called variance. The square
root, *σ*, is called the standard deviation of
*x*. The standard deviation has important physical meaning. For
example, the following figure shows the probability density function (which
describes the likelihood that *x* takes a certain value) for a
one-dimensional Gaussian distribution with mean equal to *μ* and
standard deviation equal to *σ*. About 68% of the data fall within
the 1*σ* boundary of *x*, 95% of the data fall
within the 2*σ* boundary, and 99.7% of the data fall within the
3*σ* boundary.

Even though the Gaussian distribution assumption is the dominant assumption in engineering applications, there exist systems whose state cannot be approximated by Gaussian distributions. In this case, non-Kalman filters (such as a particle filter) is required to accurately estimate the system state.

### Filter Design

The goal of designing a filter is to estimate the state of a system using measurements and system dynamics. Since the measurements are usually taken at discrete time steps, the filtering process is usually separated into two steps:

Prediction: Propagate state and covariance between discrete measurement time steps (

*k*= 1, 2, 3, …,*N*) using dynamic models. This step is also called flow update.Correction: Correct the state estimate and covariance at discrete time steps using measurements. This step is also called measurement update.

For representing state estimate and covariance status in different steps,
*x*_{k|k}
and *P*_{k|k}
denote the state estimate and covariance after correction at time step
*k*, whereas
*x*_{k+1|k}
and
*P*_{k+1|k}
denote the state estimate and covariance predicted from the previous time step
*k* to the current time step *k*+1.

#### Prediction

In the prediction step, the state propagation is straightforward. The filter only
needs to substitute the state estimate into the dynamic model and propagate it
forward in time as
*x*_{k+1|k}
=
*f*(*x*_{k|k}).

The covariance propagation is more complicated. If the estimation system is
linear, then the covariance can be propagated
(*P*_{k|k}→*P*_{k+1|k})
exactly in a standard equation based on the system properties. For nonlinear
systems, accurate covariance propagation is challenging. A major difference between
different filters is how they propagate the system covariance. For example:

A linear Kalman filter uses a linear equation to exactly propagate the covariance.

An extended Kalman filter propagates the covariance based on linear approximation, which renders large errors when the system is highly nonlinear.

An unscented Kalman filter uses unscented transformation to sample the covariance distribution and propagate it in time.

How the state and covariance are propagated also greatly affects the computation complexity of a filter. For example:

A linear Kalman filter uses a linear equation to exactly propagate the covariance, which is usually computationally efficient.

An extended Kalman filter uses linear approximations, which require calculation of Jacobian matrices and demand more computation resources.

An unscented Kalman filter needs to sample the covariance distribution and therefore requires the propagation of multiple sample points, which is costly for high-dimensional systems.

#### Correction

In the correction step, the filter uses measurements to correct the state estimate through measurement feedback. Basically, the difference between the true measurement and the predicted measurement is added to the state estimate after it is multiplied by a feedback gain matrix. For example, in an extended Kalman filter, the correction for the state estimate is given by:

$${x}_{k+1|k+1}={x}_{k+1|k}+{K}_{k}({y}_{k+1}-h({x}_{k+1|k}))$$

As mentioned,
*x*_{k+1|k}
is the state estimate before (priori) correction and
*x*_{k+1|k+1}
is the state estimate after (posteriori) correction.
*K*_{k} is the
Kalman gain governed by an optimal criterion,
*y*_{k} is the true
measurement, and
*h*(*x*_{k+1|k})
is the predicted measurement.

In the correction step, the filter also corrects the estimate error covariance.
The basic idea is to correct the probabilistic distribution of *x*
using the distribution information of
*y*_{k+1}. This is
called the posterior probability density of *x* given
*y*. In a filter, the prediction and correction steps are
processed recursively. The flowchart shows the general algorithms for Kalman
filters.

### Estimation Filters in Sensor Fusion and Tracking Toolbox

Sensor Fusion and Tracking Toolbox™ offers multiple estimation filters you can use to estimate and track the state of a dynamic system.

#### Kalman Filter

The classical Kalman filter (`trackingKF`

) is the optimal filter for linear systems with Gaussian
process and measurement noise. A linear estimation system can be given as:

$$\begin{array}{l}{x}_{k+1}={A}_{k}{x}_{k}+{w}_{k}\\ \text{\hspace{0.17em}}\text{\hspace{0.17em}}\text{\hspace{0.17em}}{y}_{k}={H}_{k}{x}_{k}+{v}_{k}\end{array}$$

Both the process and measurement noise are assumed to be Gaussian, that is:

$$\begin{array}{l}{w}_{k}~N(0,{Q}_{k})\\ {v}_{k}~N(0,{R}_{k})\end{array}$$

Therefore, the covariance matrix can be directly propagated between measurement steps using a linear algebraic equation as:

$${P}_{k+1|k}={A}_{k}{P}_{k|k}{A}_{k}^{T}+{Q}_{k}$$

The correction equations for the measurement update are:

$$\begin{array}{l}{x}_{k+1|k+1}={x}_{k+1|k}+{K}_{k}({y}_{k}-{H}_{k}{x}_{k+1|k})\\ {P}_{k+1|k+1}=(I-{K}_{k}{H}_{k}){P}_{k+1|k}\end{array}$$

To calculate the Kalman gain matrix
(*K*_{k}) in each
update, the filter needs to calculate the inverse of a matrix:

$${K}_{k}={P}_{k|k-1}{H}_{k}^{T}{\left[{H}_{k}{P}_{k|k-1}{H}_{k}^{T}+{R}_{k}\right]}^{-1}$$

Since the dimension of the inverted matrix is equal to that of the estimated state, this calculation requires some computation efforts for a high dimensional system. For more details, see Linear Kalman Filters.

#### Alpha-Beta Filter

The alpha-beta filter (`trackingABF`

) is a suboptimal filter applied to linear systems. The
filter can be regarded as a simplified Kalman filter. In a Kalman filter, the Kalman
gain and covariance matrices are calculated dynamically and updated in each step.
However, in an alpha-beta filter, these matrices are constant. This treatment
sacrifices the optimality of a Kalman filter but improves the computation
efficiency. For this reason, an alpha-beta filter might be preferred when the
computation resources are limited.

#### Extended Kalman Filter

The most popular extended Kalman filter (`trackingEKF`

) is modified from the
classical Kalman filter to adapt to the nonlinear models. It works by linearizing
the nonlinear system about the state estimate and neglecting the second and higher
order nonlinear terms. Its formulations are basically the same as those of a linear
Kalman filter except that the
*A*_{k} and
*H*_{k} matrices in the Kalman filter are
replaced by the Jacobian matrices of
*f*(*x*_{k}
) and
*h*(*x*_{k}):

$$\begin{array}{l}{A}_{k}={\frac{\partial f({x}_{k})}{\partial {x}_{k}}|}_{{x}_{k|k-1}}\\ {H}_{k}={\frac{\partial h({x}_{k})}{\partial {x}_{k}}|}_{{x}_{k|k-1}}\end{array}$$

If the true dynamics of the estimation system are close to the linearized dynamics, then using this linear approximation does not yield significant errors for a short period of time. For this reason, an EKF can produce relatively accurate state estimates for a mildly nonlinear estimation system with short update intervals. However, since an EKF neglects higher order terms, it can diverge for highly nonlinear systems (quadrotors, for example), especially with large update intervals.

Compared to a KF, an EKF needs to derive the Jacobian matrices, which requires the system dynamics to be differentiable, and to calculate the Jacobian matrices to linearize the system, which demands more computation assets.

Note that for estimation systems with state expressed in spherical coordinates,
you can use `trackingMSCEKF`

.

#### Unscented Kalman Filter

The unscented Kalman filter (`trackingUKF`

) uses an unscented
transformation (UT) to approximately propagate the covariance distribution for a
nonlinear model. The UT approach samples the covariance Gaussian distribution at the
current time, propagates the sample points (called sigma points) using the nonlinear
model, and approximates the resulting covariance distribution assumed to be Gaussian
by evaluating these propagated sigma points. The figure illustrates the difference
between the actual propagation, the linearized propagation, and the UT propagation
of the uncertainty covariance.

Compared to the linearization approach taken by an EKF, the UT approach results in
more accurate propagation of covariance and leads to more accurate state estimation,
especially for highly nonlinear systems. UKF does not require the derivation and
calculation of Jacobian matrices. However, UKF requires the propagation of
2*n*+1 sigma points through the nonlinear model, where
*n* is the dimension of the estimated state. This can be
computationally expensive for high dimensional systems.

#### Cubature Kalman Filter

The cubature Kalman filter (`trackingCKF`

) takes a slightly different approach than UKF to generate
2*n* sample points used to propagate the covariance
distribution, where *n* is the dimension of the estimated state.
This alternate sample point set often results in better statistical stability and
avoids divergence which might occur in UKF, especially when running in a
single-precision platform. Note that a CKF is essentially equivalent to a UKF when
the UKF parameters are set to *α* = 1, *β* = 0,
and *κ* = 0. See `trackingUKF`

for the definition of
these parameters.

#### Gaussian-Sum Filter

The Gaussian-Sum filter (`trackingGSF`

) uses the weighted sum of multiple Gaussian distributions
to approximate the distribution of the estimated state. The estimated state is given
by a weighted sum of Gaussian states:

$${x}_{k}={\displaystyle \sum _{i=1}^{N}{c}_{k}^{i}{x}_{k}^{i}}$$

where *N* is the number of Gaussian states
maintained in the filter, and
*c*_{k}^{i}
is the weight for the corresponding Gaussian state, which is modified in each update
based on the measurements. The multiple Gaussian states follow the same dynamic
model as:

$${x}_{k+1}^{i}=f({x}_{k}^{i},{w}_{k}^{i}),\text{}\text{\hspace{0.05em}}\text{\hspace{0.05em}}\text{\hspace{0.05em}}\text{for}i=1,2,\dots ,N.$$

The filter is effective in estimating the states of an incompletely observable estimation system. For example, the filter can use multiple angle-parametrized extended Kalman filters to estimate the system state when only range measurements are available. See Tracking with Range-Only Measurements for an example.

#### Interactive Multiple Model Filter

The interactive multiple model filter (`trackingIMM`

) uses multiple Gaussian filters to track the position of a
target. In highly maneuverable systems, the system dynamics can switch between
multiple models (constant velocity, constant acceleration, and constant turn for
example). Modelling the motion of a target using only one motion model is difficult.
A multiple model estimation system can be described as:

$$\begin{array}{l}{x}_{k+1}^{i}={f}_{i}({x}_{k}^{i},{w}_{k}^{i})\\ \text{\hspace{0.17em}}\text{\hspace{0.17em}}\text{\hspace{0.05em}}\text{\hspace{0.05em}}{y}_{k}^{i}={h}_{i}({x}_{k}^{i},{v}_{k}^{i})\end{array}$$

where *i* = 1, 2, …, *M*, and
*M* is the total number of dynamic models. The IMM filter
resolves the target motion uncertainty by using multiple models for a maneuvering
target. The filter processes all the models simultaneously and represents the
overall estimate as the weighted sum of the estimates from these models, where the
weights are the probability of each model. See Tracking Maneuvering Targets for an
example.

#### Particle Filter

The particle filter (`trackingPF`

) is different from the Kalman family of filters (EKF and
UKF, for example) as it does not rely on the Gaussian distribution assumption, which
corresponds to a parametric description of uncertainties using mean and variance.
Instead, the particle filter creates multiple simulations of weighted samples
(particles) of a system's operation through time, and then analyzes these particles
as a proxy for the unknown true distribution. A brief introduction of the particle
filter algorithm is shown in the figure.

The motivation behind this approach is a law-of-large-numbers argument — as the number of particles gets large, their empirical distribution gets close to the true distribution. The main advantage of a particle filter over various Kalman filters is that it can be applied to non-Gaussian distributions. Also, the filter has no restriction on the system dynamics and can be used with highly nonlinear system. Another benefit is the filter’s inherent ability to represent multiple hypotheses about the current state. Since each particle represents a hypothesis of the state with a certain associated likelihood, a particle filter is useful in cases where there exists ambiguity about the state.

Along with these appealing properties is the high computation complexity of a particle filter. For example, a UKF requires propagating 13 sample points to estimate the 3-D position and velocity of an object. However, a particle filter may require thousands of particles to obtain a reasonable estimate. Also, the number of particles needed to achieve good estimation grows very quickly with the state dimension and can lead to particle deprivation problems in high dimensional spaces. Therefore, particle filters have been mostly applied to systems with a reasonably low number of dimensions (for example robots).

### How to Choose a Tracking Filter

The following table lists all the tracking filters available in Sensor Fusion and Tracking Toolbox and how to choose them given constraints on system nonlinearity, state distribution, and computational complexity.

Filter Name | Supports Nonlinear Models | Gaussian State | Computational Complexity | Comments |

Alpha-Beta | Low | Suboptimal filter. | ||

Kalman | ✓ | Medium Low | Optimal for linear systems. | |

Extended Kalman | ✓ | ✓ | Medium | Uses linearized models to propagate uncertainty covariance. |

Unscented Kalman | ✓ | ✓ | Medium High | Samples the uncertainty covariance to propagate the sample points. May become numerically unstable in a single-precision platform. |

Cubature Kalman | ✓ | ✓ | Medium High | Samples the uncertainty covariance to propagate the sample points. Numerically stable. |

Gaussian-Sum | ✓ | ✓ (Assumes a weighted sum of distributions) | High | Good for partially observable cases (angle-only tracking for example). |

Interacting Multiple Models (IMM) | ✓ Multiple models | ✓ (Assumes a weighted sum of distributions) | High | Maneuvering objects (which accelerate or turn, for example) |

Particle | ✓ | Very High | Samples the uncertainty distribution using weighted particles. |

## References

[1] Wang, E.A., and R. Van Der
Merwe. "The Unscented Kalman Filter for Nonlinear Estimation." *IEEE 2000
Adaptive Systems for Signal Processing, Communications, and Control
Symposium.* No. 00EX373, 2000, pp. 153–158.

[2] Fang, H., N. Tian, Y. Wang, M.
Zhou, and M.A. Haile. "Nonlinear Bayesian Estimation: From Kalman Filtering to a Broader
Horizon." *IEEE/CAA Journal of Automatica Sinica.* Vol. 5, Number
2, 2018, pp. 401–417.

[3] Arasaratnam, I., and S.
Haykin. "Cubature Kalman Filters." *IEEE Transactions on automatic
control.* Vol. 54, Number 6, 2009, pp. 1254–1269.

[4] Konatowski, S., P. Kaniewski,
and J. Matuszewski. "Comparison of Estimation Accuracy of EKF, UKF and PF Filters."
*Annual of Navigation.* Vol. 23, Number 1, 2016, pp.
69–87.

[5] Darko, J. "Object Tracking: Particle Filter with Ease." https://www.codeproject.com/Articles/865934/Object-Tracking-Particle-Filter-with-Ease.